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1 .  Abstract 
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The  goal  of  this  Trident  project  was  to  develop  an  independent  control  scheme  to  allow  a 
team  of  autonomous  tugboats  to  move  a  large  disabled  vessel,  such  as  a  barge,  to  a  desired 
position  and  orientation.  Independence  refers  to  the  extent  to  which  each  tugboat’s  actions  were 
free  from  knowledge  of  the  locations  and  actions  of  other  tugboats.  Performance  of  the  team 
was  quantified  by  measuring  the  positional  error  and  time  required  to  affect  the  motion,  while 
respecting  maximum  power  constraints  on  the  thrust.  Applications  of  the  project  include 
difficult  or  dangerous  tasks  such  as  moving  disabled  vessels  or  vessels  “not  under  command” 
through  hostile  or  dangerous  areas,  and  transportation  of  large  objects  such  as  marine 
construction  equipment,  off-shore  bases,  drilling  platforms,  and  sonar  arrays. 

Although  it  would  be  ideal  to  increase  both  the  independence  and  performance  of  the 
system,  it  must  be  realized  that  by  increasing  one  of  these,  the  other  is  typically  degraded.  In 
order  to  measure  performance,  a  control  strategy  (the  baseline)  was  designed  that  required  the 
attachment  points  of  all  tugboats  to  be  known.  However,  this  architecture  was  not  desirable, 
since  it  was  less  independent  of  system  knowledge.  In  contrast,  to  allow  for  the  elimination  of 
known  tugboat  location,  an  adaptive  control  strategy  was  developed  which  resulted  in 
degradation  of  performance.  These  two  Scenarios  were  explored  and  in  the  course  of  solving 
them,  the  tradeoff  between  performance  and  independence  was  quantified. 

To  the  author’s  knowledge,  this  is  the  first  study  of  its  kind  and  complexity.  Although 
previous  work  has  studied  adaptive  control  of  a  multi-input  and  multi-output  system,  its  extent 
and  focus  was  not  close  to  this  research.  Each  tugboat  used  on-line  adaptive  control  methods  to 
compensate  for  the  unknown  actions  of  other  swann  members.  The  analysis  was  verified 
through  simulation.  In  addition,  an  experimental  proof-of-concept  device  was  built  and  in-water 
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experiments  were  used  to  validate  the  results.  An  incremental  approach  to  experiment  design 
was  used  to  mitigate  the  challenges  of  in-water  experimentation. 

Keywords:  Adaptive  Control,  Swann  Robotics,  Parameter  Identification,  and  Automatic 
Control 
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The  term  “swarm  robotics”  refers  to  using  a  group  of  small  and  relatively  inexpensive 
robots  to  complete  complex  tasks  through  communication  and  coordination  rather  than  through 
task-specific  tooling  of  a  single  more  sophisticated  robot.  A  swarm’s  decision  making  occurs  in 
a  decentralized  or  distributed  fashion  (i.e.,  there  is  no  central  lead  robot)  much  like  a  swarm  of 
ants  or  bees.  Each  member  of  the  group  decides  its  own  actions  based  on  the  information 
received,  either  through  sensors  or  explicit  communication  with  other  members  of  the  swarm. 
Alone,  each  of  the  swann  members  is  incapable  of  performing  the  task  successfully,  but  the 
whole  is  far  more  than  the  sum  of  the  parts.  The  advantages  of  swarm  robots  are  many, 
including  increased  robustness  and  survivability  (due  to  decentralized  decision  making),  lower 
cost,  and  increased  mission  adaptability  [1], 


Figure  1:  Swarm  of  autonomous  tugboats  (ovals)  manipulating  a  disabled  ship  to  port. 

The  goal  of  this  Trident  project  was  to  design  a  control  and  coordination  strategy  to  allow 
a  swarm  of  autonomous  tugboats  to  manipulate  a  barge  or  disabled  ship  as  depicted  in  Figure  1. 
This  application  was  ideal  for  swarm  robotics  since  it  is  impossible  for  a  single  boat  to  complete 
the  task  due  to  such  aspects  as  thrust  limitations  of  each  swarm  member.  Applications  of  the 
project  include  difficult  or  hazardous  tasks  such  as  moving  disabled  vessels  or  vessels  “not  under 
command”  through  hostile  or  dangerous  areas,  and  transportation  of  large  objects  such  as  marine 
construction  equipment,  off-shore  bases,  drilling  platforms,  and  sonar  arrays.  Knowledge  gained 
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by  research  from  this  project  will  benefit  the  Navy  in  the  following  ways:  tugboat  manpower 


reduction,  semi-automation  of  a  communication  intensive  and  potentially  deadly  evolution,  and 
reduced  cost.  In  addition,  knowledge  gained  from  this  project  will  give  a  single  operator  total 
control  over  the  team  of  tugboats,  rather  than  the  Navy’s  current  arrangement  of  the  pilot 
coordinating  the  actions  of  each  tugboat  crew  through  radio  communication.  Also,  control 
schema  developed  during  this  project  will  have  possible  applications  in  other  areas  of  swarm 
robotics  such  as  object  manipulation  with  land-based  robots,  manipulation  with  space-based 
robots,  and  micro-manipulation  with  small  scale  robots. 

Because  swarms  do  not  have  a  centralized  decision  maker  or  lead  robot,  any  information 
an  individual  robot  needed  to  know  about  other  members  of  the  swarm  in  order  to  complete  its 
task  must  come  through  sensor  information  or  wireless  communication.  However,  strong 
dependence  on  wireless  links  decreases  independence  by  making  the  system  more  susceptible  to 
interference,  jamming,  noise,  or  loss  of  a  swann  member.  Truly  distributed  operation  would  not 
require  any  information  passing  between  the  swarm  members  or  preexisting  knowledge  about  the 
placement  of  the  swarm  members,  offering  the  ultimate  Scenario  with  regards  to  independence. 
On  the  other  hand,  performance  of  the  swarm  generally  improved  with  more  shared  knowledge 
and  coordination,  such  as  each  boat  knowing  the  exact  position  and  intention  of  every  other  boat. 
However,  this  architecture  was  not  desirable,  since  it  is  less  independent  of  tugboat  position 
inaccuracies,  the  failure  of  a  single  tugboat  or  loss  of  the  wireless  network.  Although  it  would  be 
ideal  to  increase  both  the  independence  and  perfonnance  of  the  system,  it  must  be  acknowledged 
that  by  increasing  one  of  these,  the  other  is  typically  degraded.  A  central  challenge  in  this  area 
was  to  maximize  performance  of  the  team  without  sacrificing  independence.  This  Trident 
project  specifically  dealt  with  the  control  aspects  inherent  in  swarm  manipulation  of  a  barge. 
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Success  was  measured  by  the  time  required  to  move  the  large  vessel  from  starting  point  to  finish 
using  different  levels  of  data  exchange  (known  information).  The  project’s  goal  was  to 
systematically  “decentralize”  the  control  strategy  from  perfect  knowledge  (all-to-all  exchange  of 
position  and  thrust  information  over  the  wireless  network)  to  the  most  extreme  level  of 
decentralization  that  was  possible  (no  messages  exchanged  between  tugboats). 

6.1  Problem  Formulation 

The  swann  motion  was  conducted  in  two  phases. 

Phase  1.  Each  tugboat  will  establish  physical  contact  with  the  barge  by  moving  to  a 
desirable  point  around  the  barge’s  hull. 

Phase  2.  Each  tugboat  will  use  a  combination  of  infonnation  gathered  from  sensors  or 
communication  with  its  peers  to  calculate  its  thrust  magnitude  and  direction  in  order  to 
move  the  barge  to  its  desired  position  and  orientation. 

In  this  project,  Phase  1  has  already  occurred,  so  the  focus  will  be  directed  to  Phase  2.  In 
Phase  2,  It  was  assumed  that,  each  tugboat  knows: 

•  Its  own  location  and  orientation  with  respect  to  the  barge, 

•  Its  thrust  capabilities  (maximum  magnitude  and  direction  range), 

•  The  current  location  of  center  of  mass  and  orientation  of  the  barge, 

•  The  desired  location  of  the  center  of  mass  and  orientation  of  the  barge,  and 

•  The  physical  properties  of  the  barge  such  as  geometry,  displacement/weight,  drag 
coefficient,  and  added  mass. 

In  addition,  it  was  assumed  that: 

•  Each  tugboat  was  securely  attached  to  the  barge  and  no  slipping  was  occurring, 

•  Each  of  the  tugboats  was  identical  in  its  minimum/maximum  thrust  capabilities, 
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•  The  mass  and  drag  coefficients  of  a  tugboat  was  negligible  when  compared  to  that  of  the 
barge, 

•  The  barge  was  disabled  (not  powered),  and 

•  The  tugboats  were  equipped  with  a  method  of  sending  peer-to-peer  messages  to  other 
tugboats  (if  needed). 

Within  this  project,  we  defined  Performance  and  Independence  in  the  subsequent  manner  in 
order  to  quantify  a  controller’s  efficacy.  Performance  was  measured  by  detennining  the  amount 
of  time  it  took  the  tugboat  swarm  to  move  the  barge  from  its  initial  position  to  its  final  settling 
position  taking  into  account  positional  error  and  thrust  conservation.  Independence  was  a 
measure  of  control  strategy  decentralization  and  was  measured  by  determining  the  extent  to 
which  each  controller  was  free  from  the  knowledge  of  the  locations  and  actions  of  the  other 
tugboats  in  order  to  manipulate  the  barge.  Independence  was  quantified  in  the  controller’s 
derivation  by  detennining  the  terms  that  could  be  estimated  by  the  controller,  rather  than 
measured. 

In  order  to  explore  the  trade-off  between  performance  and  independence,  control 
strategies  utilizing  three  levels  of  given  information  were  investigated.  For  each  Scenario,  the 
goal  was  to  move  the  barge  to  a  desired  position  and  orientation.  Each  tugboat  had  to  compute 
the  magnitude  and  direction  of  its  thrust,  knowing  all  of  the  quantities  listed  above  in  addition  to 
the  infonnation  below: 

•  Scenario  I  (known  tugboat  locations):  Each  tugboat  knew  the  number  of  swarm 
members  in  contact  with  the  barge,  their  positions,  the  barge’s  hydrodynamic  drag,  and 
their  thrust  directions  but  did  not  require  knowledge  of  the  other  tug’s  thrust  magnitude. 
This  Scenario  was  considered  the  performance  baseline  as  we  expected  to  obtain  the  best 


results. 
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•  Scenario  II  (unknown  hydrodynamic  drag):  Each  tugboat  knew  the  number  of  swarm 
members  in  contact  with  the  barge,  their  positions,  and  their  thrust  directions  but  did  not 
know  hydrodynamic  drag  and  other  swarm  member’s  thrust  magnitude.  The 
hydrodynamic  drag  estimates  were  then  updated  on-line  in  real  time  to  properly 
manipulate  the  barge. 

•  Scenario  III  (unknown  positions):  Each  tugboat  knew  the  number  of  swarm  members  in 
contact  with  the  barge  and  hydrodynamic  drag  but  did  not  know  other  swarm  member’s 
thrust  magnitude,  thrust  direction,  and  position.  As  it  turned  out,  some  knowledge  of  the 
sign  of  each  location  parameter  was  required.  This  knowledge  was  mainly  needed  to 
determine  how  the  thrust  of  each  tugboat  affected  the  orientation  of  the  system  by 
determining  the  direction  of  the  net  torque.  In  essence,  each  tugboat  knew  qualitative 
information  about  how  its  actions  were  going  to  affect  the  whole  system  but  did  not  know 
the  quantitative  location  of  the  other  tugboats  and  could  adapt  its  location  estimates  in 
real  time  to  properly  manipulate  the  barge. 

The  three  Scenarios  presented  above  differ  from  the  original  four  Scenarios  proposed 
because  in  the  course  of  study  it  was  found  that  thrust  magnitude  between  tugboats  was  not 
coupled.  Coupling  meant  that  the  thrust  magnitude  of  one  tugboat  depended  on  the  thrust  output 
of  another  tugboat.  In  each  of  the  controllers  presented  below,  the  thrust  calculation  of  one 
tugboat  did  not  depend  upon  the  thrust  output  of  another  tugboat  but  was  purely  a  function  of  the 
tugboat  location,  dynamic  parameters,  and  positional  error.  The  original  four  problem 
formulation  Scenarios  are  given  below: 

•  Original  Scenario  I  (all-to-all  exchange):  Each  tugboat  knows  the  number  of  swarm 
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members  in  contact  with  the  barge,  their  positions,  and  their  thrust  magnitude  and 
direction  (most  centralized).  This  Scenario  is  similar  to  a  vessel  equipped  with  fixed 
thruster  pods  and  therefore  can  be  considered  as  the  performance  baseline. 

•  Original  Scenario  II  (known  positions  and  orientations):  Each  tugboat  knows  the 
number  of  swarm  members  in  contact  with  the  tugboat,  their  positions,  and  their  thrust 
direction  and  does  not  know  other  swann  member’s  thrust  magnitude. 

•  Original  Scenario  III  (known  positions):  Each  tugboat  knows  the  number  of  swarm 
members  in  contact  with  the  barge  and  their  positions  along  the  hull  and  does  not  know 
the  other  swarm  member’s  thrust  direction  and  magnitude. 

•  Original  Scenario  IV  (truly  decentralized):  Each  tugboat  only  knows  the  number  of 
swarm  members  in  contact  with  the  barge. 

As  explained  above,  the  de-coupled  system  allowed  the  four  original  Scenarios  to  be 
simplified  into  three  new  Scenarios.  Also,  it  was  found  that  Original  Scenario  IV  could  not  be 
solved  in  a  closed  fonn  expression.  Some  a  priori  knowledge  of  tugboat  locations  was  needed  to 
properly  control  the  barge/tugboat  system.  This  knowledge  was  needed  to  make  sure  the 
tugboats  were  pushing  in  the  correct  direction  to  properly  influence  the  rotation  of  the 
barge/tugboat  system. 

6.2  Related  Work 

Swarm  control  is  a  very  active  area  of  research  [2,  3,  4].  Similarly,  other  Trident  Scholars 
have  examined  swarm  control  problems.  In  Bishop’s  work,  various  behavior-based  control 
techniques  are  combined  to  create  a  novel  controller  designed  to  search  for  mines  [5].  In 
Esposito’s  work,  the  problem  of  maintaining  connectivity  of  a  wireless  network  for  a  swann  of 
land  based  robots  was  addressed  [6].  However,  all  works  focus  on  position  control  of  the  swarm 
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to  perform  tasks  such  as  searching,  reconnaissance,  and  traveling  to  a  goal  position.  In  this 
Trident  project,  the  swarm  will  operate  on  the  dynamic  level  where  forces  and  torques  will  be 
generated  in  order  to  move  an  object. 

A  second  area  of  active  research  is  “robot  pushing,”  first  analyzed  for  a  single  robot  [7]. 
Dynamically  manipulating  objects  using  two  or  three  robots  was  examined  in  two  previous 
works  [8,  9].  In  both  cases,  it  is  unclear  how  to  extend  the  methodologies  to  many  robots  with 
decentralized  decision  making.  A  different  approach  to  this  problem  is  explored  primarily  using 
caging  algorithms  [10,  11,  12],  Controllers  are  designed  which  force  robots  to  surround  the 
object.  Inter-robot  spacing  is  constrained  to  be  small  enough  that  it  is  impossible  for  it  to 
“escape”,  meaning  that  as  the  robots  move,  so  must  the  object.  While  this  approach  is 
decentralized,  the  primary  drawback  is  that  it  is  strictly  for  land  based  robots.  The  problem  is 
treated  as  a  position  control  problem,  ignoring  dynamic  forces.  The  extension  to  water 
manipulation  requires  consideration  of  hydro-dynamic  forces,  drift,  and  disturbances. 

As  mentioned,  Scenario  I  will  be  considered  a  baseline  for  comparison  by  the  other 
Scenarios.  In  fact,  a  situation  close  to  Scenario  I  was  solved  both  theoretically  and 
experimentally  [13].  Fossen’s  work  investigates  a  situation  close  to  Scenario  I  with  an 
experiment  using  an  apparatus  similar  to  the  one  constructed  to  reduce  power  consumption  and 
increase  maneuverability  through  singularity  avoidance  [14].  Essentially,  this  technique 
maximizes  lever  arms  to  reduce  the  required  amount  of  power  input  by  the  thrusters  [14], 
Scenarios  I  and  II  have  been  further  explored  by  Webster  in  the  work  entitled,  “Optimum 
allocation  for  multiple  thrusters”  [15]. 

6.3  Problem  Approach 

The  three  major  parts  of  the  project  were: 
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1 .  Control  design, 

2.  Simulation,  and 

3.  Experimentation. 

The  author  initially  focused  efforts  on  the  construction  of  the  experimental  vessel  due  to 
its  inherent  tendency  to  take  more  time  to  fabricate  than  previously  estimated.  However,  while 
focusing  on  experimentation,  the  theoretical  counterpart  of  this  project  was  not  neglected,  which 
included  both  control  design  and  simulation.  In  the  spring  semester,  the  author  devoted  the 
majority  of  time  to  developing  control  strategies  targeted  at  addressing  the  issues  of  Scenarios  I 
through  III.  After  developing  these  controllers,  they  were  subsequently  simulated  in  Matlab  to 
prove  viability,  and  further,  developed  their  practical  application  by  coding  the  controllers  onto 
the  base  station  using  Matlab  and  sending  proper  thrust  commands  to  a  slave  C  program  running 
on  the  rabbit  microcontroller  controlling  the  experimental  apparatus. 

This  report  will  detail  the  experimental  vessel  construction  and  discuss  the  performance 
verses  independence  tradeoff  of  each  controller  and  make  recommendations  pertaining  to  field- 
ability  and  reliability  of  each  controller. 

7.  Control  Design 

The  overall  control  design  consisted  of  three  major  steps:  selecting  a  representative  three 
degree  of  freedom  model  ( x,  y  lor  translation  and  <//  for  rotation),  developing  and  proving 
control  algorithms,  and  quantifying  the  tradeoff  between  performance  and  independence.  The 
author  first  selected  a  suitable  model  and  has  developed  infrastructure  code  in  which  to  simulate 
and  implement  the  designed  control  algorithms.  The  author  also  learned  how  to  derive,  prove, 
and  simulate  adaptive  update  laws  and  controllers.  Controllers  that  solved  Scenarios  I  through 


14 

III  were  also  developed,  simulated,  and  proven.  The  Scenario  I  baseline  controller  was  used  as  a 
performance  comparison  for  the  more  independent  controllers  of  Scenarios  II  and  III. 

7.1  Dynamic  Model 

Suitable  coordinate  frames  and  a  dynamic  model  were  found  in  Fossen’s  book  entitled, 
“Marine  Control  Systems”  [16].  The  following  describes  two  potential  reference  frames.  [16] 

•  ECEF  (e-frame)  The  Earth-centered  Earth-fixed  (ECEF)  reference  frame  was  affixed  to 
the  center  of  the  earth,  but  it  rotated  along  with  the  earth.  This  frame  was  analogous  to 
the  camera/fixed  frame  described  later  in  this  report.  The  camera  frame’s  origin  was 
affixed  where  the  camera  was  mounted.  The  camera  frame  has  all  of  the  properties  of  the 
ECEF  frame  except  that  it  was  not  affixed  to  the  earth’s  center.  [16] 

•  BODY  (b-frame)  this  was  the  frame  in  which  linear  and  angular  velocities  were  defined. 
Position  and  orientation  were  described  relative  to  an  inertia  reference  frame  such  as  the 
e-frame.  The  origin  of  the  b-frame  was  affixed  to  the  center  of  gravity  of  the  barge. 
Each  axis  was  defined  the  following  way  (see  Figure  2):  Xb  points  from  aft  to  fore,  yb 
points  to  starboard,  and  Zb  points  from  top  to  bottom.  This  reference  frame  was  used  in 
the  project.  [16] 

Each  of  these  reference  frames  is  displayed  in  Figure  3.  To  convert  between  reference 
frames,  homogeneous  transformation  matrices  were  utilized.  [16] 

According  to  the  established  convention  used  for  marine  vessels  named  SNAME  (1950),  the 
following  nomenclature  applied. 


Degrees  of 
Freedom 

forces  and 
moments 

linear  and  angular 
velocities 

positions  and 
Euler  angles 

1 

x-direction  motions  (surge) 

X 

u 

X 

2 

y-direction  motions  (sway) 

Y 

V 

y 

3 

z-direction  motions  (heave) 

Z 

w 

z 

4 

rotations  about  x-axis  (roll, heel) 

K 

P 

0 

5 

rotations  about  y-axis  (pitch, trim) 

M 

<7 

e 

6 

rotations  about  z-axis  (yaw) 

N 

r 

LP 

Table  1:  Dynamic  Model  Nomenclature. 


r  (yaw) 


w (heave) 


Figure  2:  SNAME  (1950)  marine  motion  variables. 
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Figure  3:  Diagram  of  reference  frames. 

Figure  3  shows  the  location  and  orientation  of  these  variables  relative  to  a  ship  schematic.  The 

marine  motion  variables  in  Figure  3  are  defined  with  respect  to  the  body-fixed  reference  frame. 

A  simplified  model  described  by  [16]  which  was  suited  for  this  project  was  the  three 

degree  of  freedom  (3-DOF)  model  for  surface  vessels.  This  model  neglected  heave,  roll,  and 

pitch  based  on  the  assumption  that  these  variables  were  small.  This  assumption  was  suitable  for 

most  ships  in  harbor  conditions  and  was  adequate  for  the  design  purposes.  The  following 

equations  describe  the  3-DOF  model  found  in  [16], 

P  =  R(y/)v 

Mv  +  Dv  +  Fd  =  Bu. 


Where: 
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«(<?)=«=,= 


cost//  -sin^  0 
sin^  cos^  0 
0  0  1 


represented  a  rotation  matrix  whose  purpose  was  to 


relate  b-frame  quantities  to  n- frame  quantities. 

•  v  =  [w  v  r]r  represented  the  linear  and  angular  velocities  measured  with  respect  to  the 
b-frame. 

•  P  =  [x  y  t//]7  denoted  the  positions  measured  with  respect  to  the  n-frame. 

•  The  M  matrix  [3x3]  described  the  mass  of  the  barge  (including  the  effects  of  added 
mass),  which  was  a  quantity  measured  experimentally  with  aid  from  the  Hydromechanics 
Laboratory. 

•  The  D  matrix  [3x3]  represented  the  effects  of  damping  in  the  surge  direction  and  was 
decoupled  from  sway  and  yaw  motion.  [14] 

•  The  vector  Fd  [3  x  1]  captured  any  disturbances  ( i.e .,  waves,  wind,  etc.).  In  order  to 
simplify  the  model,  Fd=  0  for  this  investigation. 

•  u  was  the  thrust  input  vector  [N/2  x  1]  from  the  swarm  members  (where  N  denotes  the 
number  of  tugboat  opposing  pairs).  This  matrix  gives  the  magnitude  of  each  swarm 
pair’s  thrust. 

•  The  B  matrix  [3  x  N/2]  described  thruster  configuration  which  was  dependent  on  the 
contact  location  and  orientation  of  the  other  tugboats  and  was  considered  partially 
unknown  in  Scenario  III.  This  will  be  described  in  more  detail  later. 

7.2  Control  Algorithm  Development 

In  this  portion  of  the  project,  a  suitable  control  strategy  for  each  swarm  member  was 
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designed  subject  to  the  knowledge  constraints  outlined  in  Scenarios  I  though  III  of  the  Problem 


Fonnulation.  The  development  of  these  control  strategies  represented  the  main  thrust  of  the 
research  in  this  project  during  the  spring  semester.  With  the  level  of  information  available  to 
each  swarm  member  decreasing  through  the  various  Scenarios,  the  effort  of  the  control  design 
was  focused  on  how  to  handle  this  reduction  of  information  as  the  swarm  architecture 
approached  a  decentralized  structure  while  still  positioning  and  orienting  the  disabled  barge.  To 
be  able  to  accomplish  this  task,  the  technique  of  adaptive  control  was  explored  since  this  strategy 
lends  itself  to  being  able  to  compensate  for  constant  but  unknown  system  parameters  values  [13]. 
An  issue  encountered  in  Scenario  II  and  Scenario  III  was  the  problem  of  unknown  parameter 
values.  In  Scenario  II,  the  hydrodynamic  drag  of  the  barge  was  unknown.  The  control  algorithm 
developed  used  on-line  adaptive  update  laws  to  estimate  drag  and  move  the  barge  to  the  desired 
endpoint.  Under  the  constraints  of  Scenario  III,  each  swann  member  was  only  aware  of  the 
number  of  members  in  the  swarm  and  the  hydrodynamic  drag.  Each  member  may  not  know 
where  in  relation  to  the  disabled  ship’s  center  of  mass  the  swarm  vehicle  had  attached.  This  lack 
of  knowledge  of  the  vehicle’s  attachment  point  was  captured  by  the  above  dynamic  model  in  the 
sense  that  elements  in  B  were  unknown.  If  it  was  assumed  that  no  slipping  occurred,  then  these 
unknown  parameters  were  constant.  Therefore,  the  area  of  adaptive  control  [13]  afforded 
techniques  to  compensate  for  unknown,  constant  parameter  value.  For  this  case,  the  adaptive 
controller  monitored  the  translation  and  orientation  tracking  errors  and  then  made  on-line 
adjustments  to  minimize  these  error  signals. 

To  investigate  the  developments  of  control  algorithms  for  Scenarios  I,  II,  and  III,  the 
basics  of  adaptive  control  had  to  be  researched.  In  the  following  chapters,  the  details  of  the 
derivation  and  proof  of  several  adaptive  controllers  will  be  presented;  including  a  three  degree- 
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of-freedom  controller,  a  three  degree-of-freedom  adaptive  update  law  to  compensate  for 


unknown  hydrodynamic  drag,  and  a  three  degree-of-freedom  adaptive  update  law  to  compensate 
for  unknown  tugboat  placement. 

7.3  Initial  Condition  Dependency 

One  important  observation  to  note  was  the  controllers’  dependency  on  the  initial 
conditions  of  the  vessel  before  the  control  algorithm  was  initiated.  The  time  to  the  desired 
endpoint,  ability  of  the  control  to  take  a  direct  path  to  the  endpoint,  and  the  steady-state  error  all 
depended  heavily  on  the  initial  state  of  the  vessel  in  relation  to  its  desired  endpoint.  For  the 
simulation  and  experimentation  parts  of  this  project,  the  author  chose  three  sets  of  initial 
conditions  and  endpoints  in  which  to  test  all  three  controllers.  These  sets  of  criterion  were  then 
simulated  and  experimented  with  all  three  controllers.  For  the  sake  of  brevity,  the  author  will 
only  present  the  third  set  of  initial  conditions  because  it  gave  the  best  overall  representation  of 
both  displacement  and  orientation  control.  The  other  sets  of  initial  conditions,  one  and  two,  will 
be  detailed  in  the  performance  analysis  and  conclusion  sections.  The  values  for  all  the  initial 
condition  sets  are  given  in  Table  2. 


Condition  Set 

Initial  1 

Final  1 

Initial  2 

Final  2 

Initial  3 

Final  3 

X  position  (m) 

3.58 

4.4 

2.16 

2.0 

5.58 

2.0 

Y  position  (m) 

0.80 

2.2 

2.96 

2.0 

0.30 

2.0 

Heading  (degrees) 

210 

100 

321 

270 

83.6 

90 

Table  2:  Initial  and  Final  Condition  Sets 
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7.4  Basics  of  Adaptive  Control 

As  explained  above,  adaptive  control  is  a  technique  used  to  account  for  an  unknown  but 
constant  parameter.  Adaptive  control  uses  the  error  signals  defined  in  a  system  to  constantly 
change  the  parameter  estimate  in  a  way  in  which  it  forces  the  error  signals  to  zero.  This  is  done 
by  defining  the  parameter  estimates’  update  law  in  terms  of  the  system  error.  As  the  error 
changes  the  parameter  estimate,  the  parameter  estimate  changes  the  output  of  the  controller. 
This  output  then  affects  the  error  of  the  system,  and  the  whole  iterative  process  begins  again. 
Essentially,  an  adaptive  controller  varies  the  parameter  estimate  to  see  how  it  will  affect  the  error 
of  the  system.  It  then  updates,  or  adapts,  its  parameter  estimate  based  on  how  the  system  reacts 
to  the  previous  parameter  estimate.  It  is  important  to  realize  that  an  adaptive  controller  does  not 
necessarily  find  the  actual  value  of  the  parameter.  This  is  explained  below  in  Section  7.5. 

7. 5  Persistent  Excitation 

In  some  cases,  the  adaptive  update  laws  eventually  drive  the  system  parameter  estimates 
to  their  actual  value.  It  is  important  to  note  that  this  does  not  always  happen  when  using 
adaptive  control.  The  goal  of  adaptive  control  is  to  drive  the  system’s  output,  be  it  velocity, 
acceleration,  or  position,  to  a  desired  output.  The  goal  of  adaptive  control  is  not  to  determine  the 
actual  system  parameter  that  was  being  changed  in  the  adaptive  update  law.  Adaptive  controllers 
use  whatever  system  parameter  estimate  that  is  needed  for  the  system  to  track  its  desired  output. 
The  below  derivations  never  detennined  the  actual  parameter  values  due  to  a  condition  called 
persistent  excitation  [13].  Persistent  excitation  means  that  the  input  of  the  system,  the  desired 
output,  must  have  been  constantly  moving  for  the  adaptive  update  laws  to  determine  the  actual 
system  parameters  [13].  In  the  below  examples,  all  of  the  inputs  (desired  outputs)  are  constant 
velocity.  A  constant  velocity  does  not  meet  the  condition  of  persistent  excitation;  therefore,  the 
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examples  do  not  find  the  actual  parameter  values.  The  condition  of  persistent  excitation  is  not 
important  for  the  project,  because  its  goal  is  not  to  determine  the  actual  system  parameters;  it  is 
to  follow  a  desired  track.  Persistent  excitation  does  not  affect  the  vessel’s  ability  to  track  a 
desired  output. 

8.  Controller  solving  Scenario  I  (known  tugboat  locations) 

After  a  necessary  explanation  of  the  background  work  pertaining  to  control  design, 
simulation,  and  experimental  vessel  design  and  construction,  the  author  will  now  present  the 
control  algorithm  derivations,  proofs,  simulation  results  and  experimental  results.  Each 
controller  will  be  explained  and  documented  in  a  separate  subsequent  chapter,  and  then 
performance  vs.  independence  will  be  analyzed  in  the  following  chapter  for  each  controller. 
Each  following  chapter  will  solve  Scenario  I  through  Scenario  III  which  were  detailed  earlier. 

8. 1  Controller  I  Derivation  and  Proof 

To  properly  actuate  the  vessel,  the  tugboat  placement  configuration  of  Figure  4  was 
selected  and  used  in  the  derivation,  simulation,  and  experimentation.  This  placement  allowed 
full  controllability  with  the  particular  constraints  on  the  system  that  were  defined  in  6.1. 
Particularly,  the  configuration  of  Figure  4  represents  only  one  possible  selection  from  a  large  set 
of  possible  configurations  that  could  control  the  barge.  This  swarm  configuration  is  then  used  to 
define  the  thrust  input  and  configuration  matrices  given  in  equation  (1.1).  The  commutation 
strategy  of  equation  (1.2)  is  needed  to  account  for  the  fact  the  tug  boats  can  only  exert  a  positive 


thrust  vector. 
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Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

M 

[3x3] 

Mass  of  the  vessel. 
Includes  added  mass 
and  moment  of 
inertia 

e 

[3x1] 

Positional  error 

¥ 

[lxl] 

Yaw  angle 

(measures 

orientation) 

a 

[3x3] 

Position  gain 

V 

[3x1] 

Vessel  acceleration 

Pd 

[3x1] 

Desired  velocity 

p 

[3x1] 

Global  velocity 

m 

[lxl] 

Lyapunov 

function 

D 

[3x3] 

Hydrodynamic  drag 

e 

[3x1] 

Velocity  error 

k 

[3x3] 

Gain  term 

m 

[lxl] 

Lyapunov 

function 

derivative 

V 

[3x1] 

Vessel  velocity 

Pd 

[3x1] 

Desired 

Position 

P 

[3x1] 

Global  position 

I 

[3x3] 

Identity  matrix 

r 

[3x1] 

Filtered  tracking 

error 

R 

[3x3] 

Rotation  matrix 
(converts  from 

global  to  body 
frame) 

Bs 

[3x3] 

Defined  swann 

thruster 

configuration 

rt 

[3x3] 

Transpose  of 
the  rotation 

matrix 

U, 

[3x1] 

Swarm  thrust 

magnitude  input 

K 

[3x3] 

Error  Gain 

Table  3:  Variable  Definitions  for  Known  Tugboat  Position  Example 


(1.2) 
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X 


Figure  4:  Utilized  Swarm  Configuration 

This  section  will  provide  the  derivation  of  a  control  algorithm  that  successfully 
manipulated  the  system  configuration  in  Figure  4  of  known  tugboat  locations,  number  of  swarm 
members,  thrust  directions,  and  hydrodynamic  drag.  The  goal  of  the  derived  controller  was  to 
drive  the  filtered  tracking  error  between  the  desired  system  position  and  actual  system  position  of 
the  barge  to  zero.  When  the  error  reached  zero,  this  meant  that  the  system  was  behaving  as 
desired  and  the  control  strategy  was  effective.  First,  the  author  defined  each  variable  that  was 
used  in  the  following  known  tugboat  location  controller  and  this  is  shown  in  Table  3.  To  start 
the  derivation,  the  aforementioned  system  model  was  manipulated  to  include  an  error  term 
consisting  of  the  difference  between  the  user  defined  desired  inertial  position  Pd  and  velocity  Pd 

and  the  actual  system  position  P  and  velocity  P  .  The  system  error  equations  are 
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e  =  Pd-P 

(1.3)  e  =  Pd-P 

r(t )  =  e-  ae. 

The  dynamic  system  model  that  is  given  in  (1.4)  uses  the  transformation  matrix  given  in  (1.5)  to 
convert  from  the  body  frame  to  the  global  frame.  This  is  done  by  using  the  derivative  of  (1.5), 
and  the  result  is  (1.6).  After  solving  the  transformation  matrix  forv,  the  result  is  plugged  into 
(1.6)  and  (1.7)  is  obtained  after  simplification  and  insertion  of  the  dynamic  model  for  v. 
Equation  (1.7)  is  essentially  the  global  acceleration  of  the  system  which  takes  into  account  barge 


dynamics. 

(1-4) 

Mv  =  Dv  =  BJJs 

(1.5) 

II 

d-6) 

P  =  Rv  +  Rv 

(1.7) 

P  =  -\//  xP  +  RM1  {-Dv  +  BSUS ) 

After  taking  the  double  derivative  of  the  filtered  tracking  error,  r ,  equation  (1 .8)  is  the  open-loop 
filtered  tracking  error  dynamics.  This  process  is  then  continued  by  substituting  equation  (1.7) 
into  the  error  equation  of  (1.8)  to  produce  a  rough  form  of  the  open-loop  filtered  tracking  error 
dynamics  given  in  (1.9).  To  further  transform  the  open-loop  filtered  tracking  error  dynamics  to 
the  global  frame,  equation  (1.5)  is  used.  This  previous  step  insures  that  the  velocity  used  in  the 
equation  is  the  global  velocity  obtained  by  GPS,  not  the  body  velocity  obtained  by  an  inertial 
measurement  unit.  Body  velocity  could  be  used  directly  in  the  equation;  however,  only  global 
velocity  was  available  for  the  system. 

r  -  Pd  -  P  +  ae 

r  =  Pd+i//xP  +  RM~1DV-RMlBsUs  +  ae 
r  -  Pd  +  ae  +  (//xP+  RMlDRTP-RMlBsUs 


(1.8) 

(1.9) 

(1.10) 
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The  overarching  goal  is  to  drive  the  filtered  tracking  error,  the  errors  of  both  the  position 
and  velocity,  to  zero.  If  the  filtered  tracking  error  is  zero,  then  it  can  be  shown  that 
e{t)  and e(t)  are  also  zero  which  means  that  the  vessel  has  arrived  at  the  desired  location.  In  all 
of  the  experimental  trials  this  meant  that  once  the  filtered  tracking  error  was  zero,  the  barge  was 
at  the  desired  point  with  zero  remaining  velocity.  In  order  to  drive  the  filtered  tracking  error  to 
zero,  the  system  needs  the  following: 

1 .  P  and  P  are  readily  available  for  measurement, 

2.  M  is  positive  definite  and  symmetric,  and 

3.  R  exhibits  the  following  properties:  RTR=I3,  R  =  —\j/  x  R  ,  and  ||f?||  =  1,  V  y/  . 

Each  of  the  previous  criteria  ensures  that  the  controller  avoids  singularities  and  is  solvable.  If 
any  of  the  criteria  is  not  valid,  controller  implementation  is  not  possible  since  the  inverse  of  M  is 
required.  If  the  required  matrices  can  not  be  inverted,  then  the  entire  controller  will  exhibit  a 
singularity.  For  almost  all  mechanical  systems,  M  will  have  full  rank  and  be  positive  definite. 

To  obtain  the  proper  mathematical  expression  for  the  system  input,  the  author  solved 
equation  (1.10)  for  the  thrust  input  vector,  U s .  The  result  is  now  called  Controller  I  and  is  given 

in  (1.1 1).  To  check  the  stability  of  the  system  and  make  sure  that  Controller  I  always  drives  the 
filtered  tracking  error  to  zero,  equation  (1.11)  is  substituted  back  into  the  open-loop  filtered 
tracking  error  dynamics  given  in  equation  (1.10)  and  the  expression  in  (1.12)  is  obtained. 

(1.11)  Us  =[RM-1BsJl[Pd+ae  +  Krr  +  iRxP  +  RM-1DRTP~] 

(1.12)  r  =  -K.r 

It  is  important  to  note  that  equation  (1.12)  follows  the  criteria  for  a  globally  exponentially  state 
equation  because  its  solution  is  r(t)  =  e  K,t  if  Kr>  0 .  This  criterion  ensures  that  any  presence  of 


the  filtered  tracking  error  will  force  its  value  back  to  zero  in  an  exponential  fashion.  The 
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definition  of  the  filtered  tracking  error  given  in  equation  (1.8)  ensures  that  as  the  filtered  tracking 
error  is  driven  to  zero,  the  positional  error  and  velocity  error  also  approach  zero.  Essentially,  this 
means  that  if  the  barge  in  not  where  it  is  supposed  to  be  at  the  desired  speed,  it  will  be  forced  to 
the  desired  location  and  velocity  by  the  controller  of  equation  (1.11). 

8.2  Controller  I  Simulation  Results 

The  controller  that  was  derived  in  8.1  was  tested  using  the  S-function  simulation  code 
presented  in  Section  12.1.  The  only  changes  made  to  the  S-function  shell  code  occurred  in  the 
previously  detailed  code  sections.  These  changes  entailed  entering  equations  (1.1),  (1.2),  (1.3), 
and  (1.11)  into  the  existing  S-function  dynamics  as  shown  in  Enclosure  15.1.1.  The  author  then 
entered  the  specific  control  gains,  initial  conditions,  and  desired  positions  associated  with  Initial 
Conditions  Set  Three.  Figure  5  illustrates  the  motion  of  the  disabled  barge  utilizing  the 
controller  of  equation  (1.11).  This  figure  shows  the  path  and  orientation  taken  by  the  simulated 
vessels  using  the  value  set  of  initial  conditions  three.  As  shown  below,  the  vessel  corrects  its 
heading  first  and  then  moves  to  the  desired  position  while  maintaining  the  desired  heading.  This 
plot  shows  the  vessel’s  holonomic  movement  capabilities.  Holonomic  vehicles  such  as 
hovercraft  and  differentially  driven  vehicles  have  the  capability  to  turn  on  a  point,  meaning  they 
have  a  turning  radius  of  zero.  Steered  vehicles  are  non-holonomic  by  nature,  meaning  that  they 
have  some  finite  turning  radius  and  the  output  of  the  system  can  be  path  dependent. 
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Figure  5:  Full  Controller  Simulation  Results  for  IC  -  3 


Although  the  vessel  is  holonomic,  it  does  have  a  preferred  direction  in  which  movement 
is  easiest.  As  with  most  marine  vessels,  the  small-scale  experimental  vessel’s  preferred  direction 
of  movement  is  in  the  direction  of  the  bow.  This  preferred  direction  was  not  taken  into  account 
in  the  control  algorithm.  The  controller  is  perfonning  regulation,  not  path  tracking;  therefore,  we 
can  not  influence  the  path  the  vessel  takes  to  the  endpoint.  However,  we  can  influence  the  path 
if  we  use  a  spline  trajectory.  A  spline  trajectory  is  essentially  fitting  a  third-order  polynomial 
curve  to  a  set  of  locations,  velocities,  and  times.  The  generated  spline  trajectory  used  in  this 
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fitted  third-order  polynomial  curves  for  both  the  x  and  y  directions,  and  then  the  arctangent  of 
the  x  and  y  velocities  was  used  to  determine  the  desired  orientation  angle.  This  approach 
ensured  that  the  bow  of  the  experimental  vessel  was  always  pointed  in  the  desired  direction  of 
motion.  The  author  decided  not  to  use  this  approach  because  it  required  a  predetermined  time  to 
the  endpoint.  Seeing  as  the  performance  measure  is  based  on  positional  steady  state  error  and 
time  to  the  endpoint,  the  author  decided  that  generating  a  spline  trajectory  would  be 
counterproductive  for  the  objective  of  this  project,  determining  the  tradeoff  between  system 
performance  and  independence. 

8. 3  Controller  I  Experimental  Results 

The  controller  for  known  tugboat  positions  (Controller  I)  was  implemented  on  the  small 
scale  experimental  vessel  explained  in  Section  13.1.  This  controller  was  inserted  into  the  shell 
code  explained  in  Section  13.4.2  to  obtain  the  control  package  in  enclosure  15.1.2.  The  shell 
code  had  to  be  modified  to  include  a  timer,  integrator,  and  differentiator.  To  measure  accurate 
time,  the  predefined  Matlab  function  tic  and  toe  were  used.  These  functions  are  essentially  a 
stop  watch,  tic  starts  the  watch  and  toe  measures  the  elapsed  time  from  the  last  tic.  To  integrate, 
the  trapezoidal  rule  was  used  and  this  equation  is  given  in  (1.13). 

(1.13)  ew  =  (lr(i)l«tM 

To  differentiate,  a  backwards  difference  was  taken  and  this  equation  is  given  below. 

0.14)  = 

t2  /, 

Although  the  methods  for  integration  and  differentiation  explained  above  are 
straightforward  and  generally  accurate,  their  use  in  the  control  package  was  not  ideal.  Both 
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methods  are  very  susceptible  to  inaccurate  time  readings,  inaccurate  position  measurements,  and 
slow  control  frequencies.  If  the  control  gains  were  not  tuned  correctly,  spikes  in  the  velocity 
reading  would  cause  the  system  to  go  unstable.  This  generally  happened  at  the  end  of  an 
experimental  run,  after  the  vessel  had  converged  to  the  desired  endpoint.  The  vessel  would  be  in 
the  process  of  keeping  station  on  the  desired  endpoint,  and  then  as  soon  as  an  inaccurate  velocity 
measurement  was  fed  into  the  control  algorithm  the  system  would  move  off  the  desired  endpoint 
in  an  erratic  fashion  and  out  of  the  field  of  view  of  the  camera.  This  happened  for  all  three 
controllers  because  all  depend  on  velocity  measurements  for  control.  Future  work  on  the  project 
will  include  elimination  of  the  velocity  measurements  through  the  use  of  an  observer.  An 
observer  is  essentially  a  predictive  filter.  The  observer  uses  measurable  quantities  from  the 
system  to  construct  a  measurement  for  the  unknown  state  such  as  translational  or  rotational 
velocity.  In  this  project,  the  observer  would  use  the  position  of  the  vessel,  along  with  the 
dynamic  model  of  the  vessel,  to  construct  a  measurement  for  velocity.  The  author  and  his 
advisors  currently  have  an  abstract  submitted  to  eliminate  velocity  measurements  in  Controller  I. 

Figure  6  shows  the  experimental  path  taken  by  the  vessel.  It  is  important  to  notice  the 
differences  and  similarities  between  the  simulated,  given  in  Figure  5,  and  experimental  paths. 
The  vessel’s  simulated  and  experimental  paths  are  very  similar  except  for  a  few  differences  that 
include  the  smoothness  of  the  path,  order  in  which  each  movement  is  performed,  and  time  to  the 
steady  state  position.  The  simulated  vessel  path  is  very  smooth  and  always  approaches  the 
desired  endpoint  while  the  experimental  path  has  a  few  fluctuations  and  seems  to  overshoot  the 
desired  endpoint  in  the  y  direction  until  the  almost  ninety  degree  turn.  The  reason  for  these 
disparities  could  be  the  mass  and  moment  of  inertia  matrix  used  during  simulation  and 
experimentation.  The  same  values  were  used  in  both  simulation  and  experimentation;  however, 
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these  values  were  very  rough  calculations  and  did  not  include  any  coupling  between  terms  or  any 
non-linear  terms.  These  disparities  were  expected  during  the  proposal  process,  and  as  stated 
before,  the  purpose  of  this  project  was  not  to  accurately  measure  the  hydrodynamic  properties  of 
the  vessel;  the  purpose  was  to  determine  the  tradeoffs  between  performance  and  independence. 
These  inaccuracies  in  the  model  will  not  affect  the  performance  analysis,  because  they  are 
prevalent  in  all  three  controllers.  It  is  expected  that  with  the  same  initial  conditions  and  same 
conditions  in  the  environment,  the  model  inaccuracies  remained  constant  throughout  all  of  the 
experimental  runs. 
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Figure  6:  Full  Controller  Experimental  Results  of  IC-3 
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The  differences  between  the  paths  taken  in  the  simulated  and  experimental  runs  are  also 
though  to  be  due  to  the  inaccurate  mass  and  drag  matrices.  The  simulated  path  shows  that  the 
controller  equally  weighs  each  of  the  three  objectives  which  include: 

1 .  converging  to  the  desired  point  in  the  x  direction, 

2.  converging  to  the  desired  point  in  the  y  direction,  and 

3.  converging  to  the  desired  orientation. 

This  equal  weight  on  the  above  objectives  yields  a  smooth  path  that  is  always  converging  to  the 
desired  endpoint.  The  actual  path  taken  is  not  as  smooth  as  the  simulation.  Figure  6  shows  that 
the  rates  of  convergence  for  all  three  objectives  are  not  the  same.  The  first  object  reached  is 
converging  to  the  desired  orientation.  Once  in  the  proper  orientation,  the  controller  maintains  the 
orientation  while  converging  to  the  desired  y  direction.  After  reaching  the  proper  y  direction  and 
orientation,  the  controller  maintained  the  previous  objectives  while  converging  to  the  desired 
point  in  the  x  direction.  This  discrepancy  in  convergence  rates  show  that  the  vessel  has  a 
preferred  direction  of  motion  as  stated  in  Section  8.2.  This  also  intuitively  makes  sense  due  to 
the  author’s  controller  gain  selection.  The  author  chose  the  largest  gain  for  the  term  influencing 
orientation,  the  second  largest  gain  for  the  term  influencing  y  position,  and  the  smallest  gain  for 
the  term  influencing  x  position.  The  gains  chosen  are  given  below  in  (1.15). 

"0.2  0  0  ' 

(1.15)  Kr  =  0  0.5  0 

0  0  0.6 

The  inaccuracies  in  the  model  as  compared  to  the  actual  system  meant  that  the  simulated 
vessel  reached  the  desired  endpoint  in  less  time  than  the  actual  vessel.  This  was  expected 
because  there  were  many  non-linear  forces  and  hydrodynamic  drag  effects  that  were  not  modeled 
opposing  the  motion  of  the  vessel.  Also,  the  inaccurate  mass  matrix  meant  that  the  vessel  in 
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simulation  could  turn  easier  than  the  actual  vessel.  This  is  shown  in  the  difference  of  path 


between  the  simulated  vessel  and  actual  vessel. 

9.  Controller  solving  Scenario  II  (unknown  hydrodynamic  drag) 

The  variables  used  for  the  following  controller  derivation  and  proof  are  given  in  Table  4. 
Controller  II  solves  Scenario  II  as  explain  in  Section  6.1.  Controller  II  uses  adaptive  control  to 
estimate  unknown  drag  parameters.  After  the  controller  derivation  and  proof,  simulation  and 
experimental  results  will  be  given  and  explained. 

9.1  Controller  II  Derivation  and  Proof 


Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

M 

[3x3] 

Mass  of  the  vessel. 
Includes  added  mass 
and  moment  of 
inertia 

e 

[3x1] 

Positional  error 

V 

[lxl] 

Yaw  angle 

(measures 

orientation) 

a 

[3x3] 

Position  gain 

1> 

[3x1 

Vessel  acceleration 

Pd 

[3x1 

Desired  velocity 

V 

[lxl] 

Yaw  rate 

v(0 

[lxl] 

Lyapunov 

function 

r 

[3x3] 

Adaptive  update 

gain  matrix 

Pd 

[3x1] 

Desired  acceleration 

r 

[3x1] 

Filtered  tracking 
error 

r 

[3x1] 

Filtered 
tracking  error 
derivative 

D 

[3x3] 

Hydrodynamic  drag 

e 

[3x1] 

Velocity  error 

P 

[3x1] 

Global  velocity 

V(t) 

[lxl] 

Lyapunov 

function 

derivative 

V 

[3x1] 

Vessel  velocity 

Pd 

[3x1] 

Desired 

Position 

k 

[lxl] 

Gain  term 

1 

[3x3] 

Identity 

matrix 

Bs 

[3x3] 

Vessel  thruster 

configuration 

R 

[3x3] 

Rotation  matrix 

(converts  from 

global  to  body 
frame) 

p 

[3x1] 

Global  position 

rt 

[3x3] 

Transpose  of 
the  rotation 

matrix 

Us 

[3x1] 

Swann  thrust 

magnitude  input 

K 

[3x3] 

Error  Gain 

Bs 

[3x3] 

Defined  swarm 

thruster 

configuration 

v(P) 

[3x3] 

Parameter 

regression 

matrix 

X 

[lxl] 

X  position 

y 

[lxl] 

Y  position 

9 

[3x3] 

Parameter 
estimate  vector 
derivative 

9 

[3x3] 

Parameter 

vector 

X 

[lxl] 

X  velocity 

y 

[lxl] 

Y  velocity 

D 

[lxl] 

Drag  parameter 
estimate 

9 

[3x1] 

Parameter 

estimate 

vector 

9 

[3x1] 

Parameter  error 

vector 

r 

[3x3] 

Adaptive  update 

gain 

9 

[3x1] 

Parameter  error 
vector  derivative 

Table  4:  Variable  definitions  for  Controller  II 
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To  derive  a  proper  controller  for  Scenario  II,  the  system  open-loop  filtered  error 
dynamics  determined  in  equation  (1.10)  must  be  used.  This  equation  is  restated  in  (2.1)  using 
the  variables  given  in  Table  4.  To  manipulate  the  system  so  that  the  drag  estimates  can  be 
extracted  and  compensated,  the  parameterization  given  in  equation  (2.2)  must  be  used.  In  the 
parameterization  process,  the  DRT  P  matrices  are  multiplied  out  to  vector  form.  This  vector 
form  is  then  manipulated  in  such  a  way  that  all  drag  estimate  terms  can  be  pulled  out  into  a  3x1 
vector  and  the  remaining  terms  can  be  grouped  in  a  3x3  matrix  as  shown  in  equation  (2.3).  The 
result  of  this  process  is  the  y^P^O  term.  This  tenn  is  equal  to  the  original  representation  but  is 

in  the  form  needed  to  apply  adaptive  control. 

(2.1)  r  =  Pd  +  ae  +  y/xP+  RMlDRTP-RM1BU 


A 

0 

0  " 

cos(^) 

sin(^) 

0" 

X 

Dx  cos(^)i  +  Dx  sin (y/)y 

0 

A 

0 

-sin(^) 

cos(^) 

0 

y 

= 

-D2  cos(^)i:  +  D2  sin(^)  v 

0 

0 

A. 

0 

0 

1 

DW 

Dj  cos(^)i:  +  £(  sin(^)  v 

cos(^)i:  +  sin(^)  y 

0  0" 

"A" 

(2.3) 

-D2  cos (y/)x  +  D2  sh^y ) y 

= 

0 

cos(^)  y  -  sin(^)i:  0 

A 

DW 

0 

0  if/ 

.A. 

(2.4)  r=Pd+ae  +  y/xP  +  RM~ly[P)  6  -  RM'BU 


After  the  new  open-loop  filtered  tracking  error  dynamics  term  with  the  drag  parameter 
vector  was  obtained,  it  was  used  to  solve  for  the  thrust  matrix  Us  to  find  the  controller  given  in 

equation  (2.5).  This  controller  was  then  substituted  back  into  the  open-loop  filtered  tracking 
error  dynamics  to  obtain  equation  (2.6).  After  cancellations,  equation  (2.6)  resolves  to  the 

equation  given  in  (2.7),  where  9  equals  (2.8). 

(2.5)  Us  =  [j R{y/)MXBS  ]”‘  [prf  +  ae  +  Krr  +  t/sxP  +  R(ti/)Mly(P )  0 
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(2.6) 

(2.7) 

(2.8) 


r  =  Pd+ae  +  ipxP  +  RM  lDRrP  -  RM  lB  \_R(y/)M  lBs  ]  ' 

•  Pd  +  ae  + Krr +  y/ x  P  + R(\//)M^ly^P^6~^ 

r  =  RMly[P)d-Krr 

6  =  6-6 


After  the  controller  derivation  given  above,  it  is  now  necessary  to  determine  an  adaptive 
update  law  to  find  a  suitable  value  of  6  for  which  the  controller  will  be  stable.  To  determine  this 
value,  the  author  will  use  a  Lyapunov  function  [13].  For  a  function  to  fit  this  definition,  it  must 
adhere  to  the  following  criteria: 

1 .  The  scalar  Lyapunov  function,  v(t)  >  0  . 

2.  The  time  derivative  function,  v(t)  <  0  . 

3 .  The  scalar  function,  v(t) ,  must  be  radially  unbounded  ( v(x)  — »  go  as  x  — >  oo  )[  1 3]. 
The  equation  given  in  (2.9)  fills  the  entire  above  criterion.  The  purpose  of  the  matrix  gain  given 
in  equation  (2.10)  is  to  determine  how  quickly  the  parameter  estimate  will  converge  to  their 
steady  state  value.  Generally  it  is  better  for  the  parameter  estimates  to  converge  as  quickly  as 
possible,  however,  power  and  thrust  constraints  must  be  observed.  A  balance  between  parameter 
update  and  power  constraints  must  be  found,  and  for  this  project  a  value  of  one  for  each  scalar 
gain  y  was  used. 


(2.9) 

(2.10) 


v(t)  =  -rTr  +  -0TY~x6 
w  2  2 


0  0 

Yi  0 
0  y3 


After  applying  the  properties  given  in  (2.11)  and  (2.13),  and  realizing  that  a  Lyapunov 
function  is  a  scalar,  the  equation  given  in  (2.12)  can  be  manipulated  to  the  form  given  in 
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equation  (2.14).  This  form  then  has  equation  (2.7)  substituted  into  it  to  yield  (2.15)  and  after 


some  more  manipulation,  the  form  of  equation  (2.17)  is  obtained.  This  equation  is  needed  so 
that  9  can  be  isolated  by  setting  the  terms  in  parenthesis  to  zero. 


(2.11) 

(2.12) 

(2.13) 

(2.14) 

(2.15) 

(2.16) 
(2.17) 


d(  1  ^ 


dt 


T 

—  r  r 


T  ■ 

=  r  r 


it)= 


T  •  . 

r  r  + 

-bT  r~'e 

+ 

-9TYl9 

2 

2 

-0TY^e  =  -eTTle 

2  2 

v(t)  =  rTr  +  0Tr-lS 

v(t )  =  rT  ^RMly(P)9^  -  rT Krr  +  9TYlS 
v(t)  =  -rTKrr  +  rT  RMly(P)0  +  9TYl9 
v(t)  =  -rT Krr  +  ( rT RM  ly(P)  +  9rT  l )  0 


Once  the  terms  in  parenthesis  of  (2.17)  are  set  to  zero  and  solved  for  0  the  result  of  (2.18)  is 
obtained.  Once  realizing  that  according  to  the  definition  of  adaptive  control  the  parameter  must 
be  constant  and  time-invariant,  equation  (2.18)  resolves  to  equation  (2.20)  because  0=  0. 

According  to  equation  (2.19),  if  9=  0  then  6  = -9 .  To  obtain  the  final  form  of  the  adaptive 
update  law,  equation  (2.20)  is  integrated  to  produce  (2.21). 


(2.18) 

0  =  [~rTRM-ly(P)Yj 

(2.19) 

0  =  0-9 

(2.20) 

0  =  \rTRM-\(P)Y~J 

(2.21) 

9  =  \‘0  ^rTRM~ly(P)Y ]r  dt 

(2.22) 

I 

II 
•5< 

(2.23) 

1 

S.0 

II 
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After  substituting  the  parameter  estimate  in  equation  (2.21)  into  the  filtered  tracking  error 


derivative  of  (2.7),  the  error  derivative  resolves  to  (2.22)  when  the  parameter  estimate  reaches  its 
steady-state  value.  It  is  important  to  notice  that  the  solution  of  equation  (2.22)  fits  the  criteria  of 
a  globally  asymptotically  stable  equation  (G.A.S.)  as  given  in  equation  (2.23).  A  G.A.S. 
equation  is  driven  to  zero  in  an  asymptotic  fashion,  meaning  that  the  filtered  tracking  error  r  is 
driven  to  zero  in  an  asymptotic  fashion.  Based  on  the  definition  of  the  filtered  tracking  error,  the 
position  and  orientation  errors  are  also  driven  to  zero  in  an  asymptotic  fashion.  This  means  that 
based  on  the  structure  of  the  controller  and  the  adaptive  update  law,  the  vessel  is  driven  to  the 
desired  position  and  orientation  in  an  exponential  fashion  for  all  points  in  the  workspace. 

9.2  Controller  II  Simulation  Results 

The  controller  that  was  derived  in  9.1  was  entered  in  the  modified  shell  code  explained  in 
8.1.  The  existing  structures  for  the  integrator,  differentiator,  and  timer  were  expanded  upon  to 
support  the  newly  introduced  variables  for  the  adaptive  update  law.  The  files  used  to  implement 
Controller  II  are  attached  in  enclosure  15.2.1.  The  simulation  was  run  and  the  path  taken  by  the 
vehicle  was  plotted  as  shown  in  Figure  7.  The  path  taken  by  the  vessel  is  almost  exactly  the 
same  as  the  simulated  results  for  Controller  I.  This  is  expected,  because  the  gain  on  the  adaptive 
update  law  was  set  high  and  the  initial  conditions  for  the  drag  parameters  were  very  close  to  their 
actual  values.  The  simulation  was  then  run  again  using  lower  adaptive  update  gains  and  initial 
conditions  further  away  from  the  actual  values.  As  expected,  the  performance  of  the  controller 
suffered  and  time  taken  to  reach  the  desired  endpoint  was  greater.  The  plot  from  this  experiment 
is  not  included  for  the  sake  of  brevity. 
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2  Figure  1  f^~|fP~||X~l 
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Simulated  Position  and  Orientation  for  IC-3  using  adaptive  D  controller 


X  position  (m) 


Figure  7:  Adaptive  Drag  Controller  Simulation  Results  for  IC-3 


The  author  also  experimented  with  the  use  of  different  gain  values.  There  were  gain 
values  on  the  filtered  tracking  error,  positional  error,  and  adaptive  update  laws.  As  expected, 
higher  gain  values  on  the  filtered  tracking  error  caused  the  system  to  react  quicker.  Higher  gain 
values  on  the  positional  error  gains  caused  the  system’s  location  to  affect  the  control  more  than 
the  system’s  velocity.  Higher  gain  values  on  the  adaptive  update  laws  caused  the  system 
parameters  to  converge  to  their  steady-state  values  quicker.  These  gain  values  required  the 
system  to  use  higher  control  inputs  to  affect  the  motion  of  the  system.  However,  gain  values 
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ceased  to  affect  the  motion  of  the  system  over  a  certain  value.  This  value  was  usually  around 


ten.  Any  number  greater  than  ten  generally  did  not  cause  the  system  to  move  any  faster.  This 
was  due  to  actuator  saturation,  which  was  included  in  the  simulation.  The  actuators  affecting  the 
motion  of  the  system  could  only  nominally  produce  around  two  Newtons  of  thrust.  This 
effectively  turned  the  control  algorithm  into  a  bang-bang  configuration.  Bang-bang  controllers 
essentially  have  one  level  of  input  and  they  turn  on  or  off  to  affect  the  output.  They  are  a  very 
rudimentary  form  a  control;  therefore,  high  control  gains  deteriorated  the  system’s  performance. 

Figure  8  shows  the  drag  parameter  values  verse  time  for  the  simulation  results  of 
Controller  II.  For  this  simulation,  the  initial  values  of  Di,  D2,  and  D3  were  chosen  to  be  6,  0.5, 
and  0.1  (Kg/s),  respectively  to  show  that  there  values  do  not  have  to  converge  to  the  actual 
values  in  order  for  the  controller  to  converge  to  the  desired  point.  The  actual  drag  values  given 
in  the  simulation  were/^ ,  D2 ,  D,  =  0.05,  0.05,  and  0.15  (Kg/s),  respectively.  As  shown  in  the 

figure  below,  the  parameter  estimates  never  converge  to  the  actual  values.  This  is  due  to  the 
condition  of  persistent  excitation  that  was  explained  in  Section  7.5.  The  system  did  not  provide  a 
constantly  changing  input  to  the  controller;  therefore,  the  controller  did  not  need  to  detennine  the 
parameters’  actual  values  in  order  to  drive  the  filtered  tracking  error  to  zero.  In  fact,  the  Di 
parameter  says  close  to  its  initial  value  of  6  (Kg/s),  when  its  actual  value  is  0.05  (Kg/s).  As 
shown  by  the  parameter  estimates,  it  takes  the  system  around  ten  seconds  to  converge  to  the 
desired  position,  and  this  can  be  determined  by  the  amount  of  time  it  takes  for  the  parameter 
estimates  to  reach  their  final  values. 
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Figure  8:  Adaptive  Drag  Controller  Simulation  Parameters  for  IC-3 


9. 3  Controller  II  Experimental  Results 

Figure  9  shows  the  path  taken  by  the  experimental  vessel  using  Controller  II.  Code  used 
to  implement  Controller  II  is  shown  in  enclosure  15.2.2.  It  is  interesting  to  notice  that  this  path 
is  almost  exactly  the  same  as  the  path  taken  by  Controller  I.  An  inherent  advantage  that 
Controller  II  has  during  experimentation  is  the  fact  the  drag  estimates  used  in  Controller  I  are  not 
very  accurate.  These  drag  terms  are  based  on  numbers  obtained  by  the  U.S.  Naval  Academy’s 
Hydromechanics  Laboratory  for  the  particular  hull  fonn  used  for  the  small-scale  experimental 
vessel.  While  determining  the  drag  of  the  model,  the  Hydromechanics  Laboratory  only  towed 
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looking  at  D3  it  is  easy  to  see  that  the  parameter  estimate  has  a  slight  oscillation.  This  slight 
oscillation  can  also  be  seen  in  orientation  history  of  Figure  9.  Both  Di  and  D2  reach  their  settling 
value  rather  quickly,  but  D3  looks  as  though  it  is  still  in  the  settling  process  as  the  experimental 
run  is  ended.  As  expected,  these  experimental  drag  parameters  are  different  than  the  simulated 
drag  parameters.  Also,  the  D3  parameter  seems  to  change  while  the  x  and  y  position  changes. 
This  shows  that  the  movements  in  the  x,  y,  and  y/  directions  have  some  coupling,  although  this 
coupling  is  very  small  and  it  appears  that  the  assumption  that  movement  is  decoupled  is  true. 
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Figure  10:  Adaptive  Drag  Controller  Experimental  Parameters  for  IC-3 
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10.  Controller  solving  Scenario  III  (unknown  tugboat  locations) 

As  stated  above  in  6.1,  Controller  III  solves  the  Scenario  in  which  exact  tugboat 
placement  is  unknown.  Although  exact  tugboat  placement  is  unknown  in  Controller  III,  there 
must  be  some  knowledge  of  the  system,  namely  the  signs  of  each  element  in  the  thrust 
configuration  matrix  B .  These  signs  must  be  known  so  that  B  is  full  rank,  meaning  that  its 
inverse  can  be  taken.  Iff?  is  not  full  rank,  its  inverse  will  yield  a  singularity  and  the  controller 
will  drive  the  system  unstable.  To  ensure  that  B  is  of  full  rank,  its  moving  parameters  must  be 
bounded  so  their  estimates  do  not  pass  through  zero.  If  the  estimates  pass  through  zero  or 
change  their  sign,  the  B  matrix  will  yield  a  singularity  during  inversion.  Essentially,  parameter 
estimate  signs  must  be  known  and  the  adaptive  controller  will  only  vary  the  magnitude  of  each 
parameter. 

10.1  Controller  III  Derivation  and  Proof 

To  derive  a  suitable  controller  for  Scenario  III,  the  open-loop  filtered  tracking  error 
dynamics  was  used  as  a  starting  point  and  the  terminology  given  in  Table  5  was  used.  To  find 
the  proper  form  for  the  equation  for  the  manipulation  needed  to  derive  the  adaptive  update  law, 
the  author  used  equation  (3.1)  and  added  a  virtual  control  input  as  shown  in  equation  (3.2). 
Adding  a  virtual  control  input  is  essentially  adding  zero  to  the  right  hand  side  of  the  equation. 
This  form  is  needed  to  continue  with  the  derivation.  Next,  the  author  must  solve  for  the  input  in 
terms  of  the  system’s  dynamic  properties.  This  is  done  by  solving  the  open-loop  system  model 

for  thrust;  however,  to  obtain  the  proper  controller,  one  must  ignore  the  -  RM  'BU  term  on 

the  right  hand  side  of  the  equation.  This  can  be  done  because  the  author  will  assume  perfect 
knowledge  of  the  B  parameter  for  the  moment.  This  term  will  be  accounted  for  during  the 


derivation  of  the  adaptive  update  laws.  After  solving  for  thrust  while  ignoring  the 
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RM'BU] 


term,  the  controller  is  obtained  in  (3.4). 


(3.1) 

(3.2) 


r  =  Pd  +  ae  +  i//  x  P  +  RM  ]DRr P  -  RM  'BU 
r  =  Pd  +  ae  +  ifrxP  +  RMlDRTP  -  RMlBU  +  [~  RM]BU  -RM'BU 


Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

Variable 

Definition 

M 

[3x3] 

Mass  of  the  vessel. 
Includes  added 

mass  and  moment 
of  inertia 

e 

[3x1] 

Positional  error 

V 

[lxl] 

Yaw  angle 

(measures 

orientation) 

a 

[3x3] 

Position  gain 

1> 

[3x1] 

Vessel  acceleration 

Pd 

[3x1] 

Desired  velocity 

w 

[lxl] 

Yaw  rate 

v(0 

[lxl] 

Lyapunov 

function 

M 

[lxl] 

Vessel’s  mass 

P, 

[3x1] 

Desired 

acceleration 

r 

[3x1] 

Filtered  tracking 
error 

r 

[3x1] 

Filtered 
tracking  error 
derivative 

D 

[3x3] 

Hydrodynamic 

drag 

e 

[3x1] 

Velocity  error 

P 

[3x1] 

Global  velocity 

m 

[lxl] 

Lyapunov 

function 

derivative 

V 

[3x1] 

Vessel  velocity 

Pd 

[3x1] 

Desired 

Position 

k 

[lxl] 

Gain  term 

/ 

[3x3] 

Identity 

matrix 

B 

[3x3] 

Vessel  thruster 

configuration 

R 

[3x3] 

Rotation  matrix 
(converts  from 

global  to  body 
frame) 

p 

[3x1] 

Global  position 

rt 

[3x3] 

Transpose  of 
the  rotation 
matrix 

Us 

[3x1] 

Thrust  magnitude 

Kr 

[3x3] 

Error  Gain 

Bs 

[3x3] 

Defined  swarm 

thruster 

configuration 

y(e) 

[9x9] 

Parameter 

regression 

matrix 

V 

[lxl] 

X  position 

V 

[lxl] 

Y  position 

ua,b,c 

[lxl] 

Scalar  member 

of  the  U  matrix 

9 

[9x1] 

Parameter 

vector 

X 

[lxl] 

X  velocity 

y 

[lxl] 

Y  velocity 

b 

[lxl] 

Drag  parameter 
estimate 

9 

[9x1] 

Parameter 

estimate 

vector 

9 

[9x1] 

Parameter  error 

vector 

Ky 

lxl] 

Absolute  value  of 
the  scalar 

parameter 
estimate 

b 

[9x1] 

Parameter  error 
vector  derivative 

9 

[9x1] 

Parameter 

estimate 

vector 

derivative 

Ky 

[lxll 

Scalar  member  of 

B  matrix 

j 

[lxl] 

Vessel’s  moment 
of  inertia 

B 

[3x3] 

Redefinition  of 
parameter  matrix 

sgn(hx>) 

[lxl] 

Sign  of  bx  y 

Table  5:  Variables  used  for  the  derivation  of  Controller  III. 
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RM  BU  = 


cos(y)  sin(y)  0 
-sin(y)  cos<y)  0 
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0 
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sgn  (b22) 


sin (w)ub  ,sin(^)wc 


m 


cos  (i//)ub 


m 


sgn  (b23) 
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(3-3) 


21 


23 


'32 


'33 


=  y{6)9 


To  start  the  derivation  of  an  adaptive  update  law  for  the  thrust  input  matrix,  the  elements  of 
thei?  matrix  must  be  parameterized  out  of  th qRM1BU  term.  This  long  process  is  shown  in  (3.3) 
and  the  result  is  the  definition  of  the  y{6)0  vectors.  The  0  vector  accomplishes  the  objective  set 


forth  in  10.  The  absolute  values  of  the  scalar  components  of  B  are  contained  in#.  The  value 
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of  0  is  used  to  define  the  scalar  members  of  the  actual  thrust  configuration  parameters  in  B  as 
shown  in  equation  (3.5).  Equation  (3.5)  also  shows  the  definition  of  the  filtered  tracking  error, 
and  this  definition  is  redefined  with  the  error  dynamics  including  the  adaptive  update 
parameterization  in  equation  (3.6). 

(3.4)  Us  =\R(y/)M-lBy[Pd+ae  +  Krr  +  iifxP  +  R(y/)M-xDRTP~] 

sgn  (bn)bn  •••  sgn  {bn)bn 

(3.5)  B=  :  :  ,r  =  e  +  ae 

sgn(h3|)  4,  •••  sgn(h33)  b33 

(3.6)  r  =  —krr  —  y{6)6 

The  definition  of  6  is  given  in  (3.7),  and  this  definition  is  used  to  define  the  Lyapunov 

function  given  in  (3.8).  The  expression  for  the  filtered  tracking  error  and <9  is  substituted  into 
equation  (3.8)  to  yield  (3.9).  Remember  that  that  one  of  the  central  tenants  of  adaptive  control  is 

that  the  values  of  the  parameters  must  be  constant,  therefore  6  =  —6  .  After  this,  the  Lyapunov 
function  is  further  manipulated  using  the  properties  of  scalar  derivation  to  produce  the  form 
given  in  equation  (3.11).  From  this  form,  the  terms  in  parenthesis  are  set  so  zero  so  that  a  value 

for 6 can  be  determined.  This  matrix  manipulation  process  is  shown  in  equations  (3.12)  and 

(3.13).  After  integrating  the  value  for 6,  equation  (3.14)  is  obtained.  It  is  important  to  notice 

that  once  the  adaptive  update  law  determines  the  proper  steady  state  value  for 6k  equation  (3.6) 
resolves  to  the  globally  exponential  equation  given  in  (3.15). 


(3.7) 

3b 

i 

3b 

ii 

3b 

(3.8) 

v(7)  =  —rTr  +  —  0T0 
w  2  2 

(3.9) 

v(t)  =  rT[-K-y(0)0']  +  0T(J) 

(3.10) 

(3.11) 

(3.12) 
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(3.13) 

(3.14) 

(3.15) 


v(t)  =  -K/r  -  rTy(9)9  -  ~9J 9 
v(t)  =  -K/r  -  / y{9 )  -9T)9 
§T=-rTy{9) 
bT  =  -/ y(9))T 

9  =  -v{9)Tr 
9  =  \'0[-y{9)Tr]dt 
r  =  -Kr 


10.2  Controller  III  Simulation  Results 


The  controller  that  was  derived  in  Section  10.1  was  entered  in  the  modified  shell  code  explained 
in  Section  8.1.  The  existing  structures  for  the  integrator,  differentiator,  and  timer  were  expanded 
upon  to  support  the  newly  introduced  variables  for  the  adaptive  update  law.  The  files  used  to 
implement  Controller  III  are  attached  in  enclosure  15.3.1.  The  simulation  was  run  and  the  path 
taken  by  the  vehicle  was  plotted  as  shown  in  Figure  11.  Not  surprising,  the  simulated  vessel 
took  much  the  same  path  as  the  other  two  simulations. 


The  adaptive  update  parameters  contained  in B  are  shown  in  Figure  12.  It  is  important  to 
notice  that  each  of  the  parameters  converges  rather  quickly  to  its  steady  state  value.  Although 
each  of  the  parameters  does  not  reach  the  actual  value  given  in  simulation,  its  steady  state  value 
is  close  to  the  actual  parameter.  This  could  be  due  to  the  fact  that  the  author  gave  the  adaptive 
update  laws  the  actual  values  as  initial  conditions.  Since  the  system  was  not  persistently  excited, 
the  parameters  were  not  expected  to  reach  their  actual  values.  All  of  the  parameters  are  the 
correct  signs  and  their  values  do  not  cross  zero,  therefore,  the  system  is  stable. 
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10.3  Controller  III  Experimental  Results 

Figure  13  shows  the  path  taken  by  the  experimental  vessel  using  Controller  III.  It  is 
interesting  to  notice  that  this  path  is  very  similar  to  the  paths  taken  by  Controller  I  and  Controller 
II.  The  code  used  to  implement  this  controller  is  in  enclosure  15.3.2.  This  controller  does, 
however,  have  some  oscillation  at  the  end  of  the  experimental  run.  This  oscillation  is  caused  by 
the  updating  thrust  configuration  matrix.  The  updating  thrust  configuration  matrix  affects  the 
whole  system,  but  most  notably  the  rotation  of  the  system.  Adapting  the  magnitude  of  the 
elements  in  the  B  matrix  is  essentially  changing  the  lever  arm  of  the  torque  generated  in  the 
controller’s  system  dynamics.  If  the  controller  believes  there  is  a  smaller  lever  ann  then  there 
actually  is  in  the  system,  it  will  apply  more  thrust  to  accomplish  the  task  then  needed.  This 
overshoot  in  orientation  will  then  feedback  to  the  B  matrix,  and  the  parameter  will  be  decreased 
to  better  model  the  actual  system.  This  action  is  shown  in  the  parameter  B5  in  Figure  14.  These 
values  overshoot  their  optimal  value  and  then  return  due  to  system  overshoot. 

It  is  important  to  notice  that  as  opposed  to  the  simulation,  the  B  parameters  are  still 
changing  at  the  completion  of  the  run.  This  is  due  to  the  many  un-modeled  nonlinear  forces  and 
coupling  affecting  the  ability  of  the  parameter  to  converge  quickly  to  its  proper  steady  state 
value.  In  fact  these  nonlinear  forces  are  what  cause  the  oscillation  in  the  parameter  value,  and 
this  in  turn  causes  a  oscillation  in  the  vessel’s  position.  Another  factor  that  could  account  for  the 
slow  changing  estimates  is  the  fact  that  they  had  to  be  bound  to  prevent  zero  crossing,  and  thus, 
an  unstable  system.  In  fact,  this  binding  causes  B8  to  reach  and  stay  at  the  tolerance  value.  This 
parameter  then  can  not  update  for  the  rest  of  the  experimental  run,  and  the  other  parameters  must 
now  compensate  for  the  lost  system  adaptability. 
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1 1 .  Performance  Analysis,  Independence  Analysis,  and  Controller  Comparison 

To  determine  the  performance  of  each  controller,  the  experimental  data  collected  was 
processed  using  several  metrics  that  were  developed  to  reveal  desirable  characteristics. 
Specifically,  these  characteristics  were  settling  time,  positional  error,  and  thrust  conservation. 
Minimal  settling  time  was  desired  so  that  experimental  vessel  could  reach  the  final  position  and 
orientation  as  fast  as  possible.  Minimal  positional  error  was  desired  so  that  the  experimental 
vessel  would  reach  the  specified  endpoint  as  accurately  as  possible,  while  taking  the  most  direct 
route.  Thrust  conservation  was  desired  so  that  the  experimental  vessel  could  reach  the  specified 
endpoint  using  the  least  amount  of  energy.  Each  one  of  the  performance  metrics  used  is  detailed 
in  the  following  paragraphs. 

11.1  Performance  Metrics 

11.1.1  Settling  Time 

Settling  time  was  the  first  performance  metric  developed.  Settling  time  was  defined  as 
the  time  the  vessel  took  to  reach  a  certain  distance  from  the  final  position.  The  aforementioned 
distance  was  determined  by  taking  the  normal  of  the  tolerance  vector.  The  tolerance  vector 
included  the  maximum  X-positional  error,  Y-positional  error,  and  angle  error  for  which  the 
vessel  was  considered  sufficiently  close  to  the  desired  endpoint.  An  important  detail  to  note  was 
that  the  angle  error  had  to  be  converted  from  radians  to  a  distance  so  that  units  would  agree. 
This  was  done  using  the  equation^/  ■Radius  =  Arclength  ,  where  Radius  is  half  the  length  of  the 
vessel.  The  arc  length  error  was  then  used  in  the  normal  to  define  the  proper  tolerance  vector.  In 
Table  6,  the  corresponding  tolerance  vectors  and  normal  values  are  displayed. 
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Maximum 
error  and 

normal  value 

Initial 

Conditions  1 

Initial 

Conditions  2 

Initial 

Conditions  3 

X-positional 
error  (m) 

0.30 

0.11 

0.30 

Y-positional 
error  (m) 

0.30 

0.11 

0.30 

Angle  error 
(m) 

0.12 

0.12 

0.12 

Normal  (m) 

0.44 

0.20 

0.44 

Table  6:  Tolerance  vector  values  and  corresponding  normal  values  for  each  set  of  initial  conditions 

To  determine  settling  time,  the  normal  of  the  error  vector  was  determined  for  each 
control  iteration  and  then  compared  to  the  nonnal  of  the  tolerance  vector.  If  the  value  of  the 
error  normal  was  less  than  the  value  of  the  tolerance  nonnal,  the  vessel  had  reached  a  point 
sufficiently  close  to  the  desired  endpoint.  This  did  not  necessarily  mean  that  the  vessel  had 
settled  at  the  desired  endpoint  as  it  could  oscillate  out  of  the  this  position,  therefore,  five 
consecutive  control  iterations  meeting  the  above  criteria  was  required.  Once  this  happen,  the 
vessel  had  settled  and  the  time  corresponding  to  the  control  iteration  was  saved.  This  time  was 
then  defined  as  the  settling  time.  Enclosure  15.4  shows  the  code  used  to  determine  the 
controller’s  settling  time.  Each  controller’s  performance,  based  on  settling  time,  is  given  in 
Table  7. 


Initial  Conditions  1 

Initial  Conditions  2 

Initial  Conditions  3 

Average 

Controller  I 

(Full) 

53.60 

85.72 

46.85 

62.06 

Controller  II 
(Adaptive 

Drag) 

53.27 

56.63 

49.23 

53.04 

Controller  III 
(Adaptive  B) 

95.18 

88.70 

50.15 

78.01 

Table  7:  Settling  Time  (seconds)  for  each  Controller. 
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Table  7  shows  a  very  interesting  result.  It  shows  that  Controller  II  has  the  best 


performance  when  using  settling  time  as  a  metric.  Controller  II  actually  settles  faster  than 
Controller  I  in  two  of  the  three  sets  of  initial  conditions,  contrary  to  what  was  expected  at  the 
beginning  of  the  project.  This  will  be  discussed  in  detail  below.  As  expected,  Controller  III  has 
the  worst  perfonnance  when  settling  time  is  used  as  a  metric. 

11.1.2  Positional  Error 

The  next  performance  metric  developed  was  positional  error.  Positional  error  is 
important  because  it  is  a  good  measure  of  the  efficiency  of  the  path  taken  to  the  desired  endpoint. 
The  less  positional  error  over  the  path,  the  more  direct  the  route  taken.  The  controller  is  also 
penalized  for  any  steady  state  error  through  this  metric,  therefore,  the  lower  the  positional 
performance  metric,  the  less  total  positional  error.  The  positional  error  metric  and  settling  time 
metric  are  closely  related  in  that  positional  error  is  actually  determined  while  finding  the  settling 
time.  The  positional  error  metric  is  defined  in  equation  (4.1). 

xd~x 

yd-y 

(yd  -y/)- Radius 

Essentially,  the  normal  of  the  positional  error  was  calculated  and  integrated  for  each  control 
iteration.  To  find  the  average  positional  error,  this  number  was  then  divided  by  the  number  of 
control  iterations.  Enclosure  15.4  shows  the  code  written  to  implement  this  process.  Each 
controller’s  performance,  based  on  positional  error,  is  given  in  Table  8. 


(4.1) 


f?ll  ||2 

PM  =  \\error\\  dt  Where,  error  = 
Jo  N  ii 
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Initial  Conditions  1 

Initial  Conditions  2 

Initial  Conditions  3 

Average 

Controller  I 

(Full) 

0.83 

0.82 

1.54 

1.06 

Controller  II 
(Adaptive 

Drag) 

0.90 

0.58 

1.71 

1.06 

Controller  III 
(Adaptive  B) 

1.09 

0.56 

1.86 

1.17 

Table  8:  Positional  Error  (meters)  for  each  Controller. 


Table  8  shows  that  Controllers  I  and  II  have  the  same  performance  when  it  comes  to 
positional  error.  This  is  interesting  as  it  is  contrary  to  the  original  hypothesis.  Controller  I  was 
expected  to  outperfonn  both  Controllers  II  and  III.  Reasons  for  this  discrepancy  will  be 
discussed  below.  Controller  III  has  the  worst  performance,  using  positional  error  as  a  metric. 
This  finding  supports  the  original  hypothesis. 

11.1.3  Thrust  Conservation 

The  last  performance  metric  developed  was  thrust  conservation.  This  metric  essentially 
calculates  the  average  thrust  used  per  control  iteration.  Thrust  conservation  was  used  as  a 
performance  metric  because  thrust  directly  correlates  to  energy.  The  less  thrust  created  by  the 
system,  the  less  energy  that  is  used  to  move  the  system  to  the  desired  endpoint.  Minimizing 
energy  is  desirable  because  this  conserves  battery  power  and  fuel.  Although  battery  power  and 
fuel  are  not  a  consideration  in  this  project’s  experimental  vessel,  they  are  very  much  a 
consideration  in  the  real  world.  To  calculate  the  total  thrust,  equation  (4.2)  is  used. 

(4.2)  TM  =  f  (ut  +  u,  +  u3  +  u4  +u5  +  u6  )dt 

Equation  (4.2)  essentially  integrates  the  total  thrust  used  by  all  tugboats  to  manipulate  the  barge. 
Since  all  of  the  thrust  values  are  positive  due  to  the  commutation  strategy,  there  is  no  need  to 
square  or  take  the  absolute  value.  The  result  is  then  averaged  by  dividing  it  by  the  number  of 
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control  iteration.  This  gives  the  average  thrust  used  per  control  iteration.  Enclosure  15.4  shows 
the  code  written  to  implement  this  process.  Each  controller’s  performance,  based  on  thrust 
conservation,  is  given  in  Table  9. 


Initial  Conditions  1 

Initial  Conditions  2 

Initial  Conditions  3 

Average 

Controller  I 

(Full) 

1.78 

1.53 

1.26 

1.52 

Controller  II 
(Adaptive 

Drag) 

1.94 

1.34 

1.23 

1.51 

Controller  III 
(Adaptive  B) 

2.61 

1.71 

1.62 

1.98 

Table  9:  Thrust  Conservation  (Newtons)  for  each  Controller. 


Table  9  shows  that  Controller  II  has  the  best  performance  when  it  comes  to  thrust 
conservation.  This  is  contrary  to  the  original  hypothesis  that  Controller  I  would  have  the  best 
performance.  This  discrepancy  will  be  explained  below.  As  expected,  Controller  III  has  the 
worst  performance  when  it  comes  to  thrust  conservation.  This  result  supports  the  original 
hypothesis. 

11.2  Performance  Comparison 

Using  all  of  the  previously  defined  performance  metrics,  Controller  II  has  comparable  or 
better  performance  than  Controller  I.  This  is  contrary  to  the  original  hypothesis  that  performance 
would  fall  in  the  following  order  (from  best  performance  to  worst  performance): 

1 .  Controller  I, 

2.  Controller  II, 

3.  then  Controller  III. 

There  are  many  reasons  why  this  could  be  true,  however,  the  most  viable  reason  is  that 
Controller  I  relies  on  exact  knowledge  of  all  the  hydrodynamic  parameters.  Since  the  objective 
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of  this  project  was  to  develop  controllers  to  solve  the  aforementioned  Scenarios  and  determine 
their  performance  and  independence,  only  a  rough  estimate  of  the  hydrodynamic  parameters  was 
calculated.  Hydrodynamic  drag  is  inherently  difficult  to  measure  for  a  system  of  this 
complexity,  which  is  why  its  detennination  was  not  a  goal  of  this  research.  This  rough 
calculation  may  have  been  what  caused  the  performance  of  Controller  I  to  be  worse  than  the 
performance  of  Controller  II.  Since  Controller  I  required  knowledge  of  the  hydrodynamic  drag, 
and  this  knowledge  was  not  exact,  the  controller  was  not  able  to  correctly  cancel  out  the  drag 
forces.  This  gave  Controller  II  an  inherent  advantage,  because  it  did  not  need  exact  knowledge 
of  the  hydrodynamic  drag  and  was  able  to  adapt  its  estimate  of  drag.  Controller  II  was  able  to 
correctly  cancel  the  drag  forces  while  Controller  I  was  not  able  to  correctly  cancel  the  drag 
forces.  In  this  case,  freedom  from  exact  model  knowledge  proved  to  be  a  performance 
advantage. 

Controller  III,  as  expected,  had  worse  performance  than  the  other  controllers.  This  was 
expected  because  of  the  nature  of  the  parameters  it  is  updating.  Controller  III,  as  explained  in 
Section  10.1,  varies  members  of  the  B  matrix.  This  matrix  essentially  contains  where  each  of  the 
tugboat  thrusts  acts  on  the  vessel,  in  addition  to  the  length  of  the  lever  arms  for  the  orientation 
manipulation.  All  of  these  factors  have  a  great  influence  on  the  controllability  of  the  vessel.  As 
shown  above,  Controller  III  had  a  much  longer  settling  time  and  thrust  usage  and  only  a  slightly 
large  positional  error  than  the  other  two  controllers.  The  large  settling  time  and  thrust  usage  was 
due  to  oscillation  induced  in  the  controller  as  it  near  the  desired  location.  This  oscillation  was 
due  to  the  updating^  parameters.  As  the  parameters  updated,  the  vessel  oscillated  its  way  to  the 
desired  position.  This  oscillation  cause  the  controller  to  uses  much  more  thrust  and  caused  the 
settling  time  to  be  quite  long.  The  controller  may  have  reached  the  desired  point  before  the 
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settling  time;  however,  it  oscillated  out  of  the  acceptable  region  soon  thereafter  causing  the 
controller  to  have  a  longer  settling  time.  The  positional  error  was  only  slightly  larger  than  the 
other  two  controllers  because  the  oscillations  were  relatively  small. 

11.3  Independence  Analysis 

To  determine  the  Independence  of  each  controller,  its  derivation  was  used.  Specifically, 
the  derivations  in  Sections  8.1,  9.1,  and  10.1  along  with  the  Scenario  derivations  from  Section 

6.3  were  used  to  detennine  the  model  knowledge  needed  by  the  controller.  Model  knowledge  is 
the  required  information,  to  include  hydrodynamic  properties  and  placement  of  the  tugboats.  To 
characterize  the  Independence  of  a  controller,  the  number  of  unknown  variables  will  be  used. 
For  example,  Controller  I  requires  exact  model  knowledge,  therefore,  its  independence  is  zero. 
Controller  II  does  not  require  exact  knowledge  of  the  hydrodynamic  drag,  which  contains  three 
unknown  variables.  Therefore,  Controller  II’s  independence  is  three.  Controller  III  does  not 
require  exact  knowledge  of  the  thrust  configuration  matrix  f?,  which  contains  nine  unknown 
variables;  therefore,  its  independence  is  nine.  A  synopsis  of  the  controllers’  independence  is 
given  in  Table  10. 


Number  of  Unknown  Variables 

Independence  metric 

Controller  I 
(known  positions) 

0 

0 

Controller  II 
(unknown  drag) 

3 

3 

Controller  III 
(unknown  positions) 

9 

9 

Table  10:  Independence  metric  (number  of  variables)  for  each  controller 


11.4  Controller  Comparison 

In  light  of  the  comparisons  already  done  in  Sections  1 1.2  and  1 1.3,  recommendations  will 
be  given  below  for  each  controller’s  use  pertaining  to  Performance  and  Independence. 
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Controller  suitability  pertains  to  its  application  and  is  best  decided  on  a  case  by  case  basis 
depending  on  the  specific  objectives  of  the  situation.  In  this  regard,  the  suitable  controller  for  a 
particular  application  is  not  always  clear  cut  and  is  very  subjective.  Generalizations  about  the 
suitability  for  certain  applications  will  now  be  given. 

In  applications  such  as  docking  or  traversing  narrow  channels,  either  Controller  I  or 
Controller  II  would  be  the  most  suitable;  depending  on  whether  the  vessel’s  exact  hydrodynamic 
drag  is  known.  In  real  world  applications,  it  is  very  doubtful  that  exact  information  about  the 
drag  properties  of  a  vessel  would  be  known,  therefore,  Controller  II  is  the  most  suitable. 
Controller  II  was  chosen  for  docking  application  because  it  offers  the  ultimate  perfonnance  with 
regards  to  all  of  the  performance  metrics.  Specifically,  the  superiority  of  Controller  II’s 
positional  error  performance  is  important  in  docking  and  narrow  channel  applications  where 
precision  is  required. 

In  applications  where  the  exact  placement  of  tug  boats  can  not  be  guaranteed,  Controller 
III  would  yield  the  best  performance.  When  testing  Controller  I,  the  wrong  B  values  were  used 
by  accident  during  one  of  the  testing  runs.  This  controller  promptly  became  unstable  due  to  the 
fact  it  was  not  properly  distributing  thrust  to  the  correct  locations  in  order  to  manipulate  the 
barge.  When  Controllers  I  and  II  have  incorrect  thrust  configuration  values  their  performance 
not  only  suffers  but  it  can  not  be  guaranteed  that  the  vessel  will  reach  the  desired  endpoint. 
However,  if  Controller  III  has  incorrect  magnitudes  of  the  thrust  configuration  values  it  can  still 
properly  control  the  vessel.  Its  performance  is  not  comparable  when  the  other  controllers  have 
correct  knowledge  of  the  thrust  configuration;  however,  it  greatly  outperforms  the  other 
controllers  if  the  opposite  is  true. 
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In  general,  Controller  II  had  the  best  mix  of  performance  and  independence.  Controller  II 
had  the  best  experimental  performance  overall,  and  was  the  second  most  independent  controller. 
Controller  III  was  the  most  independent,  but  in  testing  it  had  the  worst  performance.  Controller  I 
was  not  independent  and  did  not  have  the  best  performance.  However,  it  is  believed  that 
Controller  I  had  an  inherent  disadvantage  due  to  the  inexact  hydrodynamic  property  values  used 
in  this  research.  This  will  be  resolved  in  future  work  by  determining  the  proper  hydrodynamic 
properties  and  then  retesting  Controller  I.  In  general,  the  order  of  best  performance  with  regards 
to  independence  is  given  below  (best  performance  to  worst  performance): 

1 .  Controller  II, 

2.  Controller  I, 

3.  then  Controller  III. 

12.  Simulation 

12.1  S-function 

Simulation  with  the  Matlab  software  was  used  to  determine  if  the  derived  controllers  and 
adaptive  update  laws  would  perfonn  as  expected.  The  main  contribution  to  the  simulation  facet 
of  the  project  during  the  fall  semester  was  developing  the  framework  to  simulate  subsequently 
developed  controllers.  The  developed  framework  used  a  built-in  feature  of  Matlab  called  the  S- 
function.  The  S-function  was  essentially  a  Matlab  m-file  that  was  built  into  a  loop  containing 
adaptable  code  to  integrating  variables.  The  S-function  was  very  easy  to  use  as  compared  to  its 
counterpart,  Simulink,  because  if  one  changed  the  model  or  controller  of  a  system,  there  was  no 
need  to  reconstruct  a  Simulink  model  to  take  the  changes  into  account.  When  using  an  S- 
function,  if  the  model  or  controller  changes,  all  one  had  to  do  was  change  two  or  three  lines  of 
code.  This  code  representation  of  Simulink  saved  the  user  hours  of  time  as  models  and 


59 

controllers  were  revised  and  simulated  continually.  An  example  of  the  base  code  of  the  S- 


function  is  given  below  in  Figure  15. 


function  [ sys , xO , str , ts ]  =  shipdynandcontrol (t, x, u, f lag) 
%switch  flag, 
case  0, 

[sys, xO, str, ts] =mdl Initial izeSizes; 
case  1, 

sys=mdlDerivatives  (t,x,u) ; 
case  2, 

sys=mdlUpdate (t, x, u) ; 
case  3, 

sys=mdlOutputs (t, x, u) ; 
case  4, 

sys=mdlGetTimeOfNextVarHit  (t,x,u) ; 
case  9, 

sys=mdlTerminate (t, x, u) ; 
otherwise 

error ([' Unhandled  flag  =  ' , num2str (flag) ] ) ; 

end 

function  [ sys , xO , str , ts ] =mdl Initial izeSizes 
sizes  =  simsizes; 


sizes . NumContStates  =  4; 
sizes . NumDiscStates  =  0; 
sizes .NumOutputs  =  10; 

sizes .Numlnputs  =  0; 

sizes . DirFeedthrough  =  1; 
sizes . NumSampleTimes  =1;  %  at 


least  one  sample  time  is  needed 


function  sys=mdlDerivatives (t, x, u) 

V  =  x  ( 1 :  2 , 1 )  ; 
theta_hat  =  x(3:4,l); 

[Vdot,  e,  Vd,  U,  theta_hat_dot ] 

AdaptiveControl_D (t, V, theta_hat) ; 

Xdot  =  Vdot; 

sys  =  [Xdot;  theta_hat_dot ;  ] ; 

%  end  mdlDerivatives 
function  sys=mdlUpdate (t, x, u) 
sys  =  [] 

%  end  mdlUpdate 

function  sys=mdlOutputs (t, x, u) 

V  =  x  ( 1 :  2 , 1 )  ; 
theta_hat  =  x(3:4,l); 

[Vdot,  e,  Vd,  U,  theta_hat_dot ] 

AdaptiveControl_D (t, V, theta_hat) ; 
sys  =  [V;  e;  Vd;  U;  theta_hat] ; 

Figure  15:  Base  S-function  code 


In  the  sample  S-function  given  above,  the  only  code  that  the  user  had  to  change  for 


different  simulations  was  the  code  highlighted  in  red  and  of  a  larger  size.  In  the  first  highlighted 
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section,  all  the  user  had  to  change  for  different  models  was  the  number  of  discontinuous  and 
continuous  states  along  with  the  number  of  inputs  and  outputs.  These  quantities  were 
determined  by  the  system  model  along  with  the  number  of  variables  that  needed  to  be  integrated. 
In  the  second  and  third  section  of  red  and  large  code,  all  the  user  needed  to  change  was  the 
definition  of  variables,  in  this  case  the  first  two  red  lines,  and  the  function  AdaptiveControlD. 
AdaptiveControlD  was  where  the  user  defined  the  specifications  of  the  system,  along  with  the 
system  model,  controller,  and  adaptive  update  law.  This  function  was  easy  to  change,  and  while 
simulating  the  different  Scenarios  with  the  experimental  apparatus  as  the  model,  all  the  user  had 
to  change  for  the  different  Scenarios  was  the  lines  of  code  containing  the  equation  of  the 
controller  and  the  equation  of  the  adaptive  update  laws. 

13.  Experimental  Vessel  Design  and  Construction 
13.1  Vessel  Design 

Experimental  vessel  design  had  somewhat  evolved  from  what  was  previously  proposed. 
Figure  16  showed  the  proposed  design  of  the  small  scale  experimental  vessel.  Essentially,  the 
proposed  vessel  consisted  of  a  hull  made  of  closed  cell  foam,  6-7  variable  angle  thruster  pods, 
and  electronics  including  an  on-board  computer  and  wireless  modem.  After  various 
modifications  to  the  original  design,  the  small  scale  experimental  vessel  took  shape  to  what  is 
shown  in  Figure  17  and  Figure  18.  Modifications  to  the  original  design  included:  using  a 
prefabricated  hull  rather  than  closed  cell  foam  and  using  fixed  angle  thruster  pods  rather  than 
variable  angle  thruster  pods.  The  reasons  for  these  deviations  are  given  in  the  following 
paragraphs. 
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Figure  16:  Original  Vessel  Schematic 


Hull  design  was  changed  based  on  the  convenience  and  availability  of  a  prefabricated 
hull.  The  original  design  used  closed  cell  foam  because  of  its  price,  availability,  and  buoyancy. 
However,  using  closed  cell  foam  has  disadvantages,  including  unrealistic  hydrodynamic 
properties  and  issues  pertaining  to  durability.  Originally,  the  benefits  of  using  closed  cell  foam 
for  hull  fabrication  outweighed  the  disadvantages.  However,  after  discovering  a  prefabricated 
fiberglass  YP  model  hull  in  the  lab,  the  author  decided  to  use  it  rather  than  closed  cell  foam. 
This  fiberglass  hull  had  all  the  benefits  of  closed  cell  foam  but  also  compensated  for  its 
disadvantages.  The  fiberglass  hull  has  hydrodynamic  properties  similar  to  an  actual  vessel,  a 
YP,  and  the  author  obtained  the  value  of  these  properties  from  the  Oceanography  Department. 
The  fiberglass  hull  was  also  very  durable,  as  the  particular  hull  had  been  used  many  times  before 
and  was  approximately  15  years  old.  This  adaptation  to  the  vessel  had  vastly  improved  its 
durability,  and  modeled  a  real  world  vessel  more  closely  than  closed  cell  foam. 
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Figure  17:  Experimental  Vessel  (Top) 


Fixed  angle  thruster  pod  design  was  used  rather  than  variable  angle  thruster  pod  design 
due  to  the  nature  of  the  control  strategy  that  will  be  implemented  on  the  vessel.  Currently,  there 
exists  a  control  strategy  using  fixed  angles  in  the  previous  work  [1].  This  strategy  was  adapted 
to  fit  the  other  design  Scenarios.  Due  to  the  fixed  angle  control  strategy,  the  author  designed  the 
bilge  pump  mounting  brackets  shown  in  Figure  17  and  Figure  18.  These  brackets  were  then 
constructed  by  the  machine  shop.  Although  the  bilge  pump  mounting  brackets  currently  have  a 
fixed  angle,  they  are  adaptable  if  one  wished  to  implement  a  variable  angle  control  design 
strategy  in  the  future.  Changes  to  implement  a  variable  angle  design  would  include  attaching  a 
hobby  servo  and  gear  assembly  to  each  bracket.  The  internals  of  the  vessel  already  have  all  the 
hardware  necessary  to  control  the  hobby  servos. 
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Figure  18:  Experimental  Vessel  (Side) 


13.2  Vessel  Internals 

The  vessel’s  internals  consisted  of  batteries  and  the  control  board  connected  with  the 
corresponding  wires  and  cables. 

13.2.1  Batteries 

The  batteries  were  two  12V  Powersonic  lead-acid  batteries  connected  in  parallel  that 
provide  power  for  the  bilge  pumps  and  various  peripheral  devices  and  one  7.2V  nickel  cadmium 
battery  pack  that  provided  power  for  low  power  devices  such  as  the  SV203  boards.  These 
batteries  were  connected  to  the  control  board  through  switches,  fuses,  and  supply  terminals  as 
shown  in  Figure  19.  Power  for  each  device  was  then  pulled  off  of  the  supply  terminals  (6V, 
12V,  and  Ground)  and  run  through  wire. 
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Figure  19:  “Control  Board”  of  the  Vessel 


13.2.2  Control  Board 

The  majority  of  the  electronics  internal  to  the  vessel  were  mounted  on  an  aluminum  sheet 
and  this  assembly  was  nicknamed  the  “control  board.”  Specifically,  the  control  board  consisted 
of  the  Serial  Expander  Rabbit  on-board  computer,  two  daisy-chained  SV203  boards,  6  RC 
controller  boards,  and  6  TD340  motor  driver  boards  as  shown  in  Figure  20.  All  of  these  parts 
were  constructed  by  the  WSE  TSD  department.  The  author’s  contribution  to  the  construction  of 
the  control  board  was  selecting,  mounting,  and  integrating  the  parts.  The  purpose  of  each  part  is 


detailed  below: 
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Control  Board  Schematic 


Figure  20:  Control  Board  Schematic 


1.  Serial  Expander  Rabbit:  The  Rabbit  microcontroller  was  a  small  computer  that  is 
mounted  on-board  the  vessel.  The  Rabbit  was  needed  to  receive  the  proper  thrust 
commands  from  the  base  station  and  distribute  them  to  the  proper  tug  boat.  The  Serial 
Expander  Rabbit  was  a  special  version  of  the  System  Engineering  Department’s  single 
board  computer  mainstay,  the  Rabbit  microcontroller.  This  microcontroller  consisted 
of  the  Rabbit  3000  microprocessor  and  5  serial  ports.  This  microcontroller  was 
different  than  other  Rabbit  microcontrollers  because  it  does  not  include  peripherals 
such  as  A/D  converts,  D/A  converts,  etc.  Instead  of  these  devices,  the  Serial  Expander 
Rabbit  included  4  more  serial  ports  which  make  it  ideal  for  the  project.  The  author  did 
not  need  the  aforementioned  peripherals;  however,  the  project  was  serial 
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communications  intensive.  This  board  provided  needed  serial  ports  without  unused 
peripherals. 

2.  SV203  Boards:  The  purpose  of  the  SV203  board  was  to  provide  a  pulse  width 
modulated  (PWM)  signal  when  given  an  input  in  the  range  of  1  to  255  servo  counts. 
This  signal  could  be  used  to  move  a  servo  motor  to  the  correct  position  or  could  be 
converted  and  amplified  to  serve  as  a  throttle  for  a  motor.  The  input  was  in  string  form, 
which  was  essentially  a  sentence  consisting  of  ASCII  characters.  The  program  on  the 
SV203  board’s  embedded  PIC  processor  decoded  the  input  string  and  then  sent  a 
corresponding  PWM  signal  to  the  specified  output  port. 

3.  RC  Controller  Boards:  The  purpose  of  the  RC  controller  board  was  to  convert  the 
position  PWM  signal  sent  out  from  the  SV203  board  to  a  continuous  speed  signal  that 
could  be  used  to  control  a  motor  driver.  This  was  done  by  the  code  on  the  board’s 
embedded  PIC  processor.  The  output  of  the  RC  controller  board  was  another  PWM 
signal;  however,  this  signal  was  continuous  and  held  until  a  new  input  was  received  by 
the  board. 

4.  TD340  Boards:  The  purpose  of  the  TD340  motor  drive  board  was  to  take  the  PWM 
speed  signal  from  the  RC  controller  board  and  convert  that  signal  to  a  DC  voltage 
capable  of  driving  a  motor.  This  DC  voltage  level  then  corresponded  to  motor  speed. 
Once  again,  low  power  signal  (control  signal)  conversion  was  done  by  the  code  on  the 
board’s  embedded  PIC  processor.  This  control  signal  then  served  as  an  input  to  the 
four  operation  amplifiers  mounted  on  the  board.  These  op-amps  then  magnified  the 
signal  to  the  correct  DC  voltage,  and  this  was  then  supply  to  the  motor.  Actual  motor 
speed  for  this  DC  voltage  level  varied  based  on  the  characteristic  of  the  motor. 


67 

However,  for  the  system,  this  DC  voltage  level  gave  approximately  the  same  motor 
speed  for  each  bilge  pump  due  to  their  similarity. 

13.3  Vision  System 

The  vision  system  of  the  experimental  vessel  was  analogous  to  the  GPS  and  compass  on 
an  actual  vessel.  This  system  was  used  to  determine  the  experimental  vessel’s  position  and 
orientation  in  the  workspace.  The  vision  system  consisted  of  the  following  items:  two  high 
intensity  LEDs  of  different  color,  a  wide  field-of-view  webcam,  a  laptop  computer  running  code 
using  Matlab’s  image  acquisition  and  processing  toolboxes,  and  two  serial  modems  that 
transmitted  the  position  and  orientation  to  the  experimental  vessel’s  onboard  computer.  The 
operation  of  the  vision  system  will  be  described  in  the  following  paragraphs. 

Orientation  and  position  were  obtained  by  tracking  two  LEDs,  mounted  on  the  top  of  the 
experimental  vessel,  with  a  webcam.  These  LEDs  were  special-ordered  because  of  their 
intensity  and  wide  viewing  angle  and  they  are  shown  in  Figure  21.  Each  LED  consumed  a  watt 
of  power  and  had  a  70  degree  field-of-view.  This  allowed  the  webcam  to  see  the  LED  even  if  it 
was  not  directly  below.  LEDs  were  chosen  as  tracking  objects  due  to  their  light  invariance. 
Tracking  the  color  of  objects  that  did  not  produce  their  own  light  was  dependent  on  the  ambient 
light  in  the  room.  To  increase  the  reliability  of  the  vision  system,  the  author  tracked  these 
different  colored  LEDs  in  a  nearly  dark  environment  to  promote  color  invariance.  The  author 
painted  the  hull  of  the  boat  a  flat  black  to  cut  down  on  glare  from  the  LEDs  and  to  also  to  ensure 
the  boat  blended  into  the  background  during  low  light  conditions.  This  also  made  sure  that  the 
camera  was  only  tracking  the  center  of  each  LED,  not  reflections  off  the  vessel’s  cover  or  the 
surrounding  water  by  ensuring  that  potential  tracking  objects  contained  a  certain  number  of 
contiguous  pixels. 
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Figure  21:  Vessel  LEDs  used  for  Light  Invariant  Tracking 


The  light  from  these  two  LEDs  was  then  captured  by  the  wide  field-of-view  webcam  that 
was  mounted  over  the  experimental  vessel.  A  wide  field-of-view  camera  was  used  to  increase 
the  size  of  the  area  in  which  the  vessel  could  operate  since  the  ceiling  height  was  fixed.  The 
image  captured  by  the  webcam  was  then  transmitted  to  a  laptop  computer  running  Matlab  code 
that  used  the  image  processing  and  acquisition  toolboxes  to  segment  and  identify  each  LED. 
This  new  image  only  showed  the  binary  image  containing  the  light  from  the  two  LEDs  and  this 
was  used  to  compute  the  vessel’s  position  and  orientation.  Thresholding  consisted  of  running 
each  pixel  in  the  image  through  a  series  of  conditional  statements  for  each  primary  color.  If  the 
pixel  met  the  criteria  of  the  conditional  statement,  then  it  was  saved  in  a  binary  image,  an  image 
showing  the  pixels  that  met  the  criteria  in  white  and  every  other  pixel  in  black.  The  position  and 
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orientation  was  sent  to  the  vessel’s  on-board  computer  through  a  pair  (send  and  receive)  wireless 
serial  modems.  Once  the  message  containing  the  vessel’s  position  and  orientation  was  received, 
the  on-board  computer  read  and  stored  these  values  for  use  by  the  control  code. 

13.4  System  Integration 

After  constructing  the  physical  components  of  the  experimental  vessel,  the  author  had  to 
integrate  the  system.  This  partly  consisted  of  getting  each  physical  and  electronic  part  of  the 
vessel  to  work  together  through  communication.  There  were  two  ways  that  the  components 
talked  to  each  other,  either  they  sent  an  electronic  signal,  such  as  a  PWM  signal,  through  wire  or 
they  sent  a  serial  communication  either  through  wire  or  wirelessly.  All  of  the  electronic  signal 
communications  were  preprogrammed  by  TSD,  so  the  only  facet  of  communication  the  author 
had  to  integrate  was  serial  communication.  The  other  aspect  of  system  integration  was 
developing  shell  code  in  which  to  run  the  developed  control  algorithms.  The  purpose  of  the  shell 
code  was  to  act  as  an  online  integrator,  clock,  and  communication  hub.  The  project’s  work  in 
serial  communications  and  shell  code  will  be  detailed  below. 

13.4.1  Serial  Communications 

Serial  Communications  were  used  to  transmit  the  vessel  its  orientation  and  position  from 
the  vision  system  and  were  also  used  to  communicate  between  the  vessel’s  on-board  computer 
and  the  motor  control  electronics.  A  serial  communication  is  essentially  a  message  that  was 
passed  over  wire  sequentially.  There  was  typically  one  wire  for  transmitting  and  one  wire  for 
receiving  between  the  two  devices  that  are  communicating  to  each  other.  Problems  inherent  to 
serial  communications  will  be  detailed  in  the  following  paragraphs. 

Difficulty  with  hardware  implementation  included  problems  with  buffers  and  serial 
cables.  All  devices  that  have  a  serial  port  also  have  a  buffer,  which  is  a  memory  location  to  store 
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incoming  and  outgoing  messages.  This  buffer  needed  to  be  cleared  before  each  communication 
session  to  ensure  that  information  left  in  the  buffer  was  not  being  sent.  If  the  buffer  was  not 
cleared,  the  whole  communication  would  be  corrupted.  The  author  ran  into  this  problem  while 
originally  trying  to  send  a  serial  communication.  This  problem  was  solved  by  including  clear 
commands  in  the  code.  Another  hardware  problem  encountered  was  using  the  correct  serial 
cable  between  devices.  Apparently  there  are  two  cables  used  in  serial  communications,  a  one-to- 
one  cable  that  is  used  to  communicate  between  a  computer  and  a  peripheral  device  and  a  cable 
that  had  the  send  and  receive  wires  crossed  to  communicate  between  computers.  A  one-to-one 
cable  was  used  between  a  computer  and  a  peripheral  device  because  the  peripheral  device 
already  crossed  the  send  and  receive  wires  within  its  hardware.  This  was  a  problem  when  trying 
to  use  the  serial  hyperlink  on  a  desktop  computer  to  determine  what  the  vessel’s  onboard 
computer  was  receiving  from  the  laptop.  The  author  solved  this  problem  by  constructing  a  wire 
that  had  the  send  and  receive  wires  crossed  to  communicate  between  the  two  computers. 

Software  problems  in  serial  communications  mainly  consisted  of  timing  problems  and 
message  passing  problems.  Timing  was  very  important  in  serial  communications  because  both 
the  sending  and  receiving  devices  must  have  been  coordinated  to  pass  the  proper  message.  If  the 
sending  or  receiving  device  stopped  sending  or  receiving  in  the  middle  of  a  message,  the 
message  would  not  be  passed  in  its  entirety,  therefore,  making  it  useless.  It  was  important  to 
remember  that,  in  the  project,  both  the  sending  and  receiving  devices  were  computers  that  also 
had  other  tasks.  These  other  tasks  could  interrupt  the  message  passing  and  inadvertently  corrupt 
the  message,  and  the  communications  program  took  this  into  account  by  using  a  handshake 
protocol.  This  protocol  communicated  between  devices  to  make  sure  that  each  was  ready  to  pass 
the  message.  Once  the  message  was  passed  it  was  sent  again  to  tell  the  computers  that  it  was 
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safe  to  go  to  other  tasks.  This  ensured  that  the  message  was  sent  in  its  entirety;  however,  it  did 
not  make  sure  the  correct  message  was  being  passed.  Stopping  extra  characters  from  being  sent 
had  consumed  a  good  amount  of  time.  For  example,  Matlab’s  serial  communication  send 
command  automatically  appended  a  new  line  feed  character.  The  experimental  vessel  was 
receiving  the  correct  message;  however,  this  message  was  always  preceded  by  the  new  line  feed 
character.  It  took  some  time  to  discover  that  this  was  an  inherent  feature  of  the  Matlab  command 
as  it  was  not  documented.  The  lesson  learned  from  this  problem  was  that  one  must  ensure  that 
the  correct  message  was  being  passed  between  devices,  because  it  was  very  easy  for  an  extra 
character  to  be  sent.  Since  serial  communications  is  sequential,  this  corrupted  every  following 
message. 

13.4.2  Shell  Code 

The  author’s  major  contribution  to  system  integration  fall  semester  had  been  to  develop 
shell  code  from  which  the  control  algorithm  would  be  implemented.  Using  the  shell  code,  the 
code  for  each  new  Scenario  was  just  a  revision  of  a  few  lines  to  incorporate  the  new  controller. 
Specifically,  the  author  had  completed  the  code  for  receiving  a  serial  communication  from  the 
vision  system  and  code  for  sending  the  proper  motor  command  to  the  peripheral  motor  throttle. 
Other  work  done  on  the  shell  code  during  the  spring  semester  included  coding  an  online 
numerical  differentiator  to  determine  vessel  speed  and  acceleration  from  the  position  coordinates 
and  vessel  rotation  rate  from  the  orientation.  This  was  done  in  the  code’s  main  loop  by  using  a 
backwards  difference  method. 

13.5  Large  Scale  Experimental  Vessel 

As  the  year  progressed,  the  author  constructed  the  control  board  of  the  vessel  first.  The 
control  board  included  all  of  the  vital  electronics  of  the  vessel,  mainly  the  on-board  computer 
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and  motor  amplifiers.  After  constructing  the  control  board,  the  author  and  his  advisors  decided 
to  increase  the  scale  of  the  experimental  vessel  to  an  actual  in-water  large  scale  vessel  consisting 
of  a  10  foot  boat  using  trolling  motors  as  thruster  pods  as  shown  in  Figure  22.  Problems  with  the 
ordering  process  resulted  in  supplies  arriving  too  late  for  implementation.  Due  to  the  order 
delays,  the  author  and  his  advisors  decided  to  pursue  two  experimental  vessels,  a  small  scale 
experimental  vessel  and  a  large  scale  on-water  experimental  vessel.  The  primary  vessel  for 
experimentation  was  the  small  scale  vessel.  The  purpose  of  the  large  scale  vessel  was  to  provide 
an  on-water  demonstration  platform.  Data  collection  for  determination  of  the  tradeoff  between 
performance  and  independence  was  provided  by  experimentation  using  the  small  scale  vessel; 
therefore,  this  part  of  the  experimentation  aspect  of  the  project  was  deferred  to  future  work. 


Figure  22:  Large  Scale  Experimental  Vessel 


14.  Conclusion 
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14.1  Contributions 

This  research  has  made  several  notable  contributions;  specifically,  to  the  field  of  Control 
Systems  Engineering  and  to  ongoing  research  at  the  United  States  Naval  Academy.  The 
deliverables  of  this  project  are  sorted  using  the  previous  categories  as  discussed  in  the  following 
paragraphs. 

14.1.1  Contributions  to  Control  Systems  Engineering 

This  research  has  made  three  major  contributions  to  the  state  of  the  art  of  control  systems 
engineering.  These  three  contributions  were  the  derivation,  proof,  simulation,  and 
experimentation  of  Controller  I;  the  derivation,  proof,  simulation,  and  experimentation  of 
Controller  III;  and  the  performance  verses  independence  analysis  of  Controllers  T-TTT.  Each  one 
of  these  contributions  will  be  detailed  in  the  following  paragraphs. 

1.  Although  previous  work  has  studied  some  aspects  of  Scenario  I,  Controller  I  is  novel 
because  it  is  the  first  control  algorithm  to  employ  unidirectional  control  inputs  for  all 
three  degrees  of  freedom  in  the  model.  Essentially,  Controller  I  is  unique  in  its 
placement  of  the  tugboats  around  the  barge  and  its  use  of  a  commutation  strategy  to 
ensure  that  tugboats  are  only  pushing  against  the  hull.  No  previous  work  has  addressed 
the  problem  of  tugboat  manipulation  of  a  barge  in  this  way.  Controller  I  is  superior  in 
some  aspects  to  the  previous  work  because  it  allows  control  of  the  barge  by  only  using 
positive  force  from  each  tugboat.  This  is  desirable  because  pushing  is  generally  more 
efficient  in  marine  applications.  Current  marine  propulsion  systems  are  vastly  more 
efficient  when  operation  in  the  positive  (pushing)  direction  as  compared  to  the  reverse 


74 

direction.  Controller  I  takes  advantage  of  this  inherent  efficiency  in  current  marine 
propulsion. 

2.  The  derivation  and  proof  of  Controller  III  in  and  of  itself  is  a  notable  contribution  to  the 
field  of  control  systems  engineering.  Controller  III  is  the  first  control  algorithm  of  its 
type,  complexity,  and  application  to  be  simulated  and  experimentally  proven.  Another 
substantial  contribution  is  the  identification  of  needed  improvements  on  Controller  III,  as 
detailed  in  Section  14.2.  This  research  essentially  developed  Controller  III  to  solve 
Scenario  III  and  then  identified  that  future  work  is  needed  to  have  a  controller  that  is 
completely  independent  on  knowledge  of  the  thrust  configuration.  An  important 
discovery  during  the  derivation  process  of  Controller  III  was  the  requirement  of  some  a 
priori  knowledge  about  the  signs  of  each  element  in  the  B  matrix.  This  requirement  was 
needed  because  Controller  III  uses  the  inverted  B  matrix  to  determine  the  proper  thrust 
allocation  to  the  tug  boats.  When  inverting  the  B  matrix,  it  is  important  that  it  maintains 
full  rank.  If  the  elements  in  the  B  matrix  are  left  unbounded,  they  could  potentially  run 
through  zero,  causing  the  inverted  matrix  to  loose  full  rank  and  the  system  to  become 
uncontrollable,  meaning  that  there  is  no  possible  solution  set  to  drive  the  filtered  tracking 
error  to  zero.  Also,  if  the  elements  in  the  B  matrix  are  left  unbounded  and  they  run 
through  zero  to  values  of  the  opposite  sign  it  is  also  probable  that  the  system  will  become 
unstable.  This  loss  of  stability  is  due  to  the  fact  that  the  lever  arm  for  the  torque  terms 
manipulating  the  vessel’s  orientation  changes  signs.  When  this  happens,  the  controller 
essentially  believes  it  is  applying  a  torque  on  the  system  to  rotate  in  one  direction,  and  in 
actuality  it  is  rotating  the  opposite  direction.  This  quickly  causes  the  system  to  loose 
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stability  and  controllability.  The  required  a  priori  knowledge  of  the  signs  of  the  B  matrix 
motivated  the  need  for  the  future  work  explained  in  Section  14.2. 

3.  The  performance  analysis  detailed  in  Section  1 1,  was  also  a  significant  contribution  to  the 
field  of  control  systems  engineering.  The  performance  and  independence  analysis 
allowed  this  research  to  make  several  recommendations  about  the  field  ability  of  each 
controller.  The  performance  analysis  section  discovered  that  the  hydrodynamic 
properties  used  in  Controller  1  were  not  accurate.  This  discovery  is  important  because  it 
is  now  apparent  that  an  adaptive  update  law  for  drag  and  other  hydrodynamic  terms  only 
helps  the  performance  of  a  controller.  Using  adaptive  control  to  account  for  inexact 
hydrodynamic  drag  measurement  may  be  more  viable  than  actually  determining  the  drag 
due  to  the  complex  process  required  to  detennine  its  correct  values.  Future  work, 
detailed  in  Section  14.2,  will  further  investigate  this  discovery  and  make  a 
recommendation  as  to  whether  or  not  adaptive  update  laws  should  be  included  in  the 
controllers  of  each  Scenario  and  Scenarios  defined  in  the  future. 

14.1.2  Contributions  to  Ongoing  Research 

Many  contributions  to  other  research  at  the  United  States  Naval  Academy  have  been 
made  by  this  project.  Specifically,  contributions  such  as  the  experimental  vessel,  Controller  II, 
simulation  infrastructure,  and  the  developed  vision  system  will  help  current  and  future  research 
of  this  topic.  These  contributions  will  be  detailed  below. 

1 .  Experimental  vessel  design  and  implementation  will  help  future  research  on  this  problem 
and  promote  development  of  Scenarios  and  controllers  by  providing  a  reliable  and  easily 
reconfigurable  platform  for  their  experimentation.  This  system  has  already  been 
designed  and  integrated  in  such  a  way  to  allow  for  growth.  As  future  controllers  are 
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developed,  the  hardware  and  code  background  already  exists  and  is  easily  modified.  This 
platform  was  designed  to  be  rugged  and  to  be  easily  reconfigurable. 

2.  Although  previous  work  has  already  used  adaptive  controllers  to  account  for  unknown 
hydrodynamic  drag,  the  development  of  Controller  II  was  instrumental  in  performance 
analysis  and  will  be  incorporated  in  future  work.  More  testing  will  be  done,  as  detailed  in 
Section  14.2,  to  future  characterize  the  specific  advantages  inherent  to  Controller  II’s 
design.  The  possibility  of  implementing  the  adaptive  update  law  developed  in  Controller 
II  on  other  Scenarios  will  be  investigated. 

3.  Much  like  the  experimental  vessel’s  contribution,  the  simulation  infrastructure  will 
contribute  significantly  to  ongoing  research.  As  detailed  in  Section  12,  the  simulation 
infrastructure  was  designed  to  be  readily  reconfigurable.  This  infrastructure  requires  only 
a  few  changes  to  successfully  simulate  a  new  controller. 

4.  The  vision  system  developed  in  this  research  and  described  in  Section  13.3  has  also 
contributed  significantly  to  ongoing  research  in  this  topic  and  others.  This  system  has 
proven  to  be  very  reliable  and  invariant  to  changes  in  ambient  light,  and  it  can  be  and 
easily  applied  to  other  research.  Although  this  vision  system  is  very  reliable, 
improvements  in  its  design  are  possible  and  are  detailed  in  Section  14.2. 

14.2  Future  Work 

Although  this  Trident  project  investigated  many  problems  associated  with  autonomous 
manipulation  of  a  barge  with  robotic  tugboats,  there  still  remains  many  areas  of  the  topic  that 
require  further  research.  Future  work  on  the  project  will  be  listed  and  explained  below  in  detail. 

1.  To  integrate  the  disparities  between  simulation  and  experimentation,  a  hardware-in- the- 
loop  (HIL)  simulation  needs  to  be  developed.  A  HIL  simulation  uses  mathematical 
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representations  of  many  complex  phenomena  in  order  to  provide  more  a  realistic  model. 
A  HIL  simulation  also  uses  the  actual  control  code  and  processor.  For  this  project,  the 
internal  subsystems  that  need  to  be  more  accurately  modeled  include:  tugboat  thrust 
output,  hydrodynamic  drag  (to  include  the  effects  of  each  tugboat  and  lateral 
movement),  and  the  mass  matrix  (to  include  added  mass  effects).  In  short,  to  have  the 
simulation  more  realistically  model  the  experiment  the  hydrodynamic  and  physical 
properties  of  the  vessel  must  be  more  closely  modeled. 

2.  One  area  of  future  work  that  is  needed  to  research  item  number  one  is  a  more  complete 
understanding  of  the  hydrodynamic  effects  of  the  experimental  vessel.  Early  in  the 
project,  it  was  thought  that,  due  to  the  system’s  slow  speed,  hydrodynamic  effects  would 
be  negligible  in  a  controlled  environment.  This  was  not  true,  because  even  in  the  very 
controlled  environment  of  the  Naval  Academy’s  tow  tank  hydrodynamics  still 
significantly  affected  the  system.  The  single  biggest  hydrodynamic  effect  that  needs  to 
be  modeled  for  a  more  accurate  system  is  hydrodynamic  drag.  Computational  fluid 
dynamics  (CFD)  could  be  used  to  more  accurately  model  the  highly  non-linear  drag 
effects  experienced  by  a  ship  moving  laterally.  Once  a  CFD  model  is  developed,  the 
drag  effects  of  each  tugboat  could  be  included  rather  than  assuming  that  the  tugboats 
have  no  effect  on  the  system’s  hydrodynamic  properties. 

3.  To  help  the  debugging  process,  work  needs  to  done  to  allow  the  MATLAB  control  files 
to  run  without  input  from  the  camera.  Current  control  files  require  a  video  input  to  run, 
and  this  is  not  very  conducive  to  testing  changes  in  the  code.  Each  control  file  needs  to 
be  changed  so  that  the  vision  system  can  be  turned  off  and  positional  data  can  be  read  in 
from  a  preexisting  file.  The  current  arrangement  is  fully  operational,  but  is  not  ideal. 
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4.  To  test  the  reliability  of  the  control  algorithms  with  environmental  disturbances,  they 
need  to  be  tested  on  an  open-water  experimental  vessel.  Currently,  a  large  scale 
experimental  vessel  is  under  construction  and  is  almost  ready  for  a  hardware-in-the-loop 
simulation.  This  vessel  currently  uses  the  same  control  code  and  basic  setup,  however, 
GPS  integration  is  planned  for  the  future.  Additionally,  this  vessel  will  soon  be  tested  in 
the  tow  tank  to  ensure  the  system  has  been  properly  integrated. 

5.  Hardware  changes  that  should  be  implemented  to  improve  the  small  scale  experimental 
vessel  include  a  wider  fleld-of-view  camera  and  replacing  the  Serial  Expander  Rabbit 
microcontroller  with  a  PC  104  microcomputer.  Currently,  the  fleld-of-view  of  the 
webcam  utilized  is  only  large  enough  to  do  very  limited  experimentation.  Quantifying 
performance  was  difficult  because  there  was  simply  not  enough  space  for  the 
performance  between  controllers  to  be  quantified.  Although  there  are  cameras  with 
wider  fields  of  view,  they  tend  to  have  more  distortion  at  the  edges  of  the  image.  This 
could  be  countered  by  either  placing  the  camera  further  above  the  water  or  by  using  a 
multi-camera  system.  Multi-camera  systems  use  the  images  from  multiple  cameras  to 
completely  cover  an  area  without  distortion  by  carefully  piecing  the  images  together. 
Multi-camera  systems  are  complicated  and  expensive  but  offer  better  coverage  of  the 
workspace.  A  camera  system  may  be  the  only  option  to  increase  the  size  of  the 
workspace  in  the  tow  tank  because  placing  the  camera  higher  above  the  water  is  not 
feasible.  Changing  the  vessel’s  on-board  computer  is  needed  because  current  code  has 
already  exceeded  the  Rabbit  microcontroller’s  memory.  The  solution  used  this  year  was 
to  move  the  control  code  off  of  the  vessel’s  on-board  computer  and  on  to  the  base 
station.  Even  though  this  solution  gave  the  same  results  as  the  previous  setup,  it  is  ideal 


79 

to  have  the  control  code  running  on-board  the  vessel.  The  original  setup  allowed  the 
control  code  to  be  processed  at  a  faster  rate  than  the  video  feed.  This  is  desirable 
because  although  new  positional  data  is  only  available  at  the  video  update  rate 
(approximately  2  Hz),  the  control  code  could  be  run  at  a  much  faster  rate  by  utilizing 
estimated  data  from  an  observer.  Observers  use  data  from  a  slow  sensor  to  estimate  the 
signal  between  updates.  Running  the  control  code  at  a  faster  rate  is  desirable  because  it 
greatly  improves  the  response  of  the  system. 

6.  Although  Controller  III  offers  a  viable  solution  to  the  problem  of  unknown  tugboat 
location,  it  still  requires  some  knowledge  of  the  system.  As  stated  in  10.1,  Controller  III 
requires  knowledge  of  the  sign  of  each  element  in  B  .  This  requirement  is  not  ideal,  as 
the  information  may  not  be  available  in  real  world  applications.  There  are  three 
solutions  to  this  problem:  Nassbaum  Gains,  root-searching  functions,  and  a  switching 
controller  [17,  18].  Nassbaum  Gains  and  root-searching  functions  continuously  change 
the  parameter’s  sign  until  they  discover  the  correct  value.  Nassbaum  gains  are  not 
robust  and  have  not  been  successfully  implemented  [17].  Root-searching  functions  do 
not  have  the  robustness  problems  of  Nassbaum  gains  but  have  only  been  solved  for 
simplified  cases,  nothing  close  to  the  complexity  of  this  system  [17].  Switching 
controllers  start  with  a  parameter  identification  phase  and  then  move  to  the  actual 
control  phase.  The  parameter  identification  phase  uses  a  special  observer  to  detennine 
the  sign  of  unknown  parameters  and  then  uses  an  adaptive  controller  much  like 
Controller  III  to  determine  the  amplitude  of  the  parameter  [18].  Switching  controllers 
offer  the  most  viable  solution;  however,  if  the  signs  of  the  parameters  are  incorrect  then 
there  is  not  way  to  control  the  system.  Future  work  will  determine  if  there  is  a  viable 
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closed-form  control  expression,  and  if  there  is  not,  will  implement  one  of  the  above 
solutions. 
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16.  Enclosures 


16.1  Controller  I  Code 

16.1.1  Controller  I  Simulation  Code 

SwarmDynKin: 

function  [ sys , xO , str , ts ]  =  SwarmDynKin (t, x, u, flag) 
switch  flag, 

O, _ 

Q - 

%  Initialization 

O, _ 

Q - 

case  0, 

[sys,xO,str,ts] =mdl Initialize Sizes; 

O, _ 

Q - 

%  Derivatives 

g, _ 

o - 

case  1, 

sys=mdlDerivatives (t, x, u) ; 

O, _ 

o - 

%  Update 

o, _ 

0 - 

case  2, 

sys=mdlUpdate (t, x, u) ; 


%  Outputs 

g, _ 

q - 


case  3, 

sys=mdlOutputs (t, x, u) ; 


g, _ 

Q - 

%  GetTimeOfNextVarHit 

g _ _ 

Q - 


case  4, 

sys=mdlGetTimeOfNextVarHit  (t, x, u)  ; 


g, _ 

Q - 

%  Terminate 

g, _ 

Q - 


case  9, 

sys=mdlTerminate (t, x, u) ; 


g, 

o 


%  Unexpected  flags 


otherwise 

error ([' Unhandled  flag  = 


end 


, num2str (flag)  ]  )  ; 
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o,  o, _ 

O  O - 

%  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-function 

O, _ 

O - 

function  [sys,xO,str,ts] =mdl Initialize Sizes 

%  Call  simsizes  for  a  sizes  structure,  fill  it  in  and  convert  it  to  a  sizes 
%  array. 

%  Note  that  in  this  example,  the  values  are  hard  coded.  This  is  not  a 
%  recommended  practice  as  the  characteristics  of  the  block  are  typically 
%  defined  by  the  S-function  parameters. 

sizes  =  simsizes; 

sizes . NumContStates  =  6; 
sizes . NumDiscStates  =  0; 
sizes .NumOutputs  =  12; 

sizes .Numlnputs  =  0; 

sizes . DirFeedthrough  =  1; 

sizes . NumSampleTimes  =  1;  %  at  least  one  sample  time  is  needed 

sys  =  simsizes (sizes) ; 

% (74 . 8) *pi/180 . 0 

xO  =  [5.59  0.5  83 . 6*pi/180  0.0  0.0  0.0]'; 

%  str  is  always  an  empty  matrix 
str  =  [ ] ; 

%  Initialize  the  array  of  sample  times 
ts  =[00]; 

%  end  mdllnitializeSizes 

Q,  O, _ 

O  O - 

%  mdlDerivatives 

%  Return  the  derivatives  for  the  continuous  states. 

O, _ 

O - 

function  sys=mdlDerivatives (t, x, u) 

%  Exchange  of  variables 
Px  =  x  ( 1 ,  1 )  ; 

Py  =  x ( 2 , 1 )  ; 
psi  =  x  (3, 1)  ; 

Vx  =  x  ( 4 , 1 )  ; 

Vy  =  x  ( 5 ,  1 )  ; 

Vpsi  =  x ( 6, 1 )  ; 

V  =  [Vx  Vy  Vpsi] '; 

P  =  [Px  Py  psi] 1 ; 


[dP,dV,U,e]  =  DynamicsControl ( P, V) ; 
sys  =  [dP;  dV]  ; 


end  mdlDerivatives 
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%  mdlUpdate 

%  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
%  requirements. 

O, 

o 


function  sys=mdlUpdate (t, x, u) 
sys  =  [ ] ; 

%  end  mdlUpdate 


%  mdlOutputs 

%  Return  the  block  outputs. 

o, 

o 


function  sys=mdlOutputs (t, x, u) 

%  Exchange  of  variables 
Px  =  x  ( 1 ,  1 )  ; 

Py  =  x ( 2 , 1 )  ; 
psi  =  x  (3, 1)  ; 

Vx  =  x  ( 4 , 1 )  ; 

Vy  =  x  ( 5 ,  1 )  ; 

Vpsi  =  x ( 6, 1 )  ; 

V  =  [Vx  Vy  Vpsi] 

P  =  [Px  Py  psi] ' ; 

[dP,dV,Us,e]  =  DynamicsControl ( P, V) ; 

O, 

o 


%  Output  Vector 

o, 

o 


sys  =  [ P ' , Us ' , e ' ] ; 


%  end  mdlOutputs 


o,  o, 
o  o 


%  mdlGetTimeOfNextVarHit 

%  Return  the  time  of  the  next  hit  for  this  block.  Note  that  the  result  is 
%  absolute  time.  Note  that  this  function  is  only  used  when  you  specify  a 
%  variable  discrete-time  sample  time  [-2  0]  in  the  sample  time  array  in 
%  mdllnitializeSizes .mdl 

O, 

o 


function  sys=mdlGetTimeOfNextVarHit (t, x, u) 


sampleTime  =1;  %  Example,  set  the  next  hit  to  be  one  second  later, 

sys  =  t  +  sampleTime; 
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%  end  mdlGetTimeOfNextVarHit 


o,  o, 
o  o 


%  mdlTerminate 

%  Perform  any  end  of  simulation  tasks. 

O, 

o 


function  sys=mdlTerminate (t, x, u) 
sys  =  [ ] ; 

%  end  mdlTerminate 


DynamicsControl : 


function  [dP,dV, U,e]  =  DynamicsControl ( P, V) 

O, _ 

O - 

%  System  Parameters  and  Matrix  Definitions 

O, _ 

O - 

%  Tugboat  locations 


rl 

= 

0.6; 

alphal 

= 

(180.0) *pi/180 . 0; 

thetal 

= 

(0.0) *pi/180 . 0; 

r2 

= 

0.27; 

alpha2 

= 

(270.0) *pi/180 . 0; 

theta2 

= 

(44.72) *pi/180 . 0; 

r3 

= 

0.19; 

alpha3 

= 

(270.0) *pi/180; 

theta3 

= 

(90.0) *pi/180 . 0; 

r4 

= 

rl; 

alpha4 

= 

(0.0) *pi/180 . 0; 

theta4 

= 

(180.0) *pi/180 . 0; 

r5 

= 

r2; 

alpha5 

= 

(90.0) *pi/180 . 0; 

theta5 

= 

(360.0) *pi/180 . 0-theta2; 

r6 

= 

r3; 

alpha6 

= 

(90.0) *pi/180 . 0; 

theta6 

= 

(270.0) *pi/180 . 0; 

%  Thrust  matrix 

B1  =  [cos(alphal)  cos(alpha2)  cos(alpha3)  cos(alpha4)  cos(alpha5) 
cos (alpha6) ] ; 

B2  =  [sin(alphal)  sin(alpha2)  sin(alpha3)  sin(alpha4)  sin(alpha5) 
sin (alpha6) ] ; 

B3  =  [rl*sin (alphal-thetal)  r2*sin (alpha2-theta2 )  r3*sin (alpha3-theta3)  ... 
r4*sin (alpha4-theta4 )  r5*sin (alpha5-theta5)  r6*sin (alpha6-theta6) ] ; 
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B  =  [Bl; 

B2  ; 

B3  ]  ; 

%  For  the  above  configuration 
Bs  =  [-1  0  0; 

0  -1  -1; 

0  -r2*cos (theta2)  0  ]; 

%  Mass  matrix 
m  =  15.5129;  %  (kg) 

Iz  =  1.5849;  %  (kgmA2) 

M  =  [m  0  0; 

0  m  0 ; 

0  0  Iz]  ; 

%  Damping  matrix 
D  =  [.05  0  0; 

0  0.05  0; 

00  .15] ; 

%%  Control 
Px  =  P  ( 1 ,  1 )  ; 

Py  =  P  (2, 1)  ; 
psi  =  P (3, 1)  ; 

%  Rotation  Matrix 
R  =  [cos (psi)  -sin (psi)  0; 
sin (psi)  cos (psi)  0; 

0  0  1  ]  ; 

Pdot  =  R*V; 

%  Desired  Trajectories 
Pd  =[2  2  (90.0) *pi/180 . 0]  '  ; 
PdDot  =  [0.0  0.0  0.0]'; 

PdDDot  =  [0.0  0.0  0.0]'; 

%  Control  gains 
gammaO  =  0.0; 

%Kp  =  0.05; 

%  Kr  =0.5; 

%  alpha  =  1.0; 

%  Kr  =  [7  0  0; 

%  0  6.0  0; 

%  003]; 

Kr  =  [2  0  0; 

0  5.0  0; 

0  0  4]; 

alpha  =  [ . 3  0  0 ; 

0  .3  0; 

0  0  .35] ; 
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%  Error  signals 
e  =  Pd-P; 
eDot  =  PdDot-Pdot; 
r  =  eDot+alpha*e; 

S  =  [0  10; 

-1  0  0; 

0  0  0  ]  ; 


Us 

inv (R*inv (M) *Bs) * ( PdDDot+alpha*eDot+P (3,1) *S*Pdot+Kr*r+R*inv (M) *D*R' *Pdot) ; 
%  Us  =  inv (R*inv (M) *Bs) * (Kp*e) ; 

ul4  =  Us  (1, 1)  ; 
u2  5  =  Us  (2 , 1)  ; 
u36  =  Us (3, 1)  ; 

ul  =  0.5* (ul4  +sqrt (ul4A2+gammaOA2) ) ; 
u4  =  0 . 5* (~ul4+sqrt (ul4A2+gammaOA2) ) ; 

u2  =  0.5* (u25  +sqrt (u25 A2+gamma0 A2 ) ) ; 
u5  =  0 . 5*  (~u25  +  sqrt (u25A2+gammaOA2) ) ; 

u3  =  0.5* (u36  +sqrt (u36A2+gammaO A2 ) ) ; 
u6  =  0 . 5*  (~u36+sqrt (u36A2+gammaOA2) )  ; 

if  (ul>2 . 4 ) 
ul  =  2.4; 

end 

if  (u4>2 . 4 ) 
u4  =  2.4; 

end 

if  (u2>l . 6) 

u2  =  1.6; 

end 

if  (u3>l . 6) 
u3  =  1.6; 

end 

if  (u5>l . 6) 
u5  =  1.6; 

end 

if  (u6>1.6) 
u  6  =  1.6; 

end 


U  =  [ul  u2  u3  u4  u5  u6] ' ; 

%%  Kinematics 
dP  =  R*V; 

%%  Swarm/Barge  System  Dynamics 
dV  =  inv (M) * (-D*V+B*U) ; 


return; 
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PlotAngle : 

close  all 
plot ( Px, Py, ' -b ' ) 
axis (  [ 0  8  0  4.4]); 
hold  on 

for  i  =  1:3:151 

plot( [Px(i)+.2*cos(psi(i) )  Px(i)-.2*cos(psi(i) ) ] ,  [Py(i)+0.2*sin(psi(i) ) 

Py(i)-0.2*sin(psi(i))],  ' —  k ' )  ; 

plot(Px(i)+.2*cos(psi(i) ) ,Py(i)+0.2*sin(psi(i) ) , ' *g ' ) ; 
plot(Px(i)-.2*cos(psi(i) ) , Py(i)-0.2*sin(psi(i) ) , ' *r ' ) 
end 

title (' Simulated  Position  and  Orientation  for  Full  Controller  using  IC3 ' ) 
xlabel('X  position  (m) ') 
ylabel('Y  position  (m) ') 

16.1.2  Controller  I  Experimental  Code 

FullControl: 

clear  M 
clear  all 

%desired  location  and  orientation  in  pixels 
x  des=input (' Input  the  desired  X  position'); 
y_des=input (' Input  the  desired  Y  position'); 
angle  des=input (' Input  the  desired  angle'); 
runtime  =  input (' Input  the  run  time'); 

O _ 

O - 

%initialize  variables  and  vessel  parameters 

o _ 

O - 

x_log  =  [];  y_log  =  [];  si_log  =  [];  Ul_log  =  [];  U2_log  =  []; 

U3_log  =  [];  U4_log  =  [];  U5_log  =  [];  U6_log  =  [];  time_log  =  []; 
x  conversion  =  7.927/640; 
y  conversion  =  4.4/480; 

Kr  =  [ . 2  0  0 ; 

0  .5  0; 

0  0  .4]; 

k_alpha  =  [.3  0  0; 

0  .3  0; 

0  0  .35] ; 

%  Kr  =  [.7  0  0; 

%  0.60; 

%  00.8]; 

%  k^alpha  =  [.17  0  0; 

%  0  .17  0; 

%  00.3]; 

al  =  180*pi/180; 
a2  =  270*pi/180; 
a3  =  270*pi/180; 

j  =  1.5849;  %using  moment  of  inertia  formula  for  a  cuboid  from 
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bhttp : / /www . diracdelta .co.uk/ science/ source/m/ o /moment s%2 Oof %20 inertia/ source 


.  html 

m  =  1 

5 

.5129 

r 

rl  = 

0 

.6; 

r2  = 

0 

.27; 

r3  = 

0 

.19; 

si  = 

0 

r 

tl  = 

0 

*pi/l 

O 

CO 

t2  = 

4 

4.72* 

pi / 180; 

t3  = 

90*pi/ 

180; 

xd  = 

X 

des  ; 

yd  = 

y. 

des  ; 

sid  = 

angle 

des*pi 

gamO 

= 

0; 

error 

age  = 

0; 

timer 

=  0; 

time 

age  = 

0; 

si  = 

[ 

0  1 

0; 

O 

\ — 1 

1 

0; 

0  0 

0]  ; 

Mass  = 


[m  0  0; 

0  m  0  ; 

0  0  j]; 


B  =  [cos(al)  cos(a2)  cos(a3); 
sin(al)  sin(a2)  sin(a3); 
rl* (cos (tl) *sin (al) -sin (tl) *cos  (al) )  .  . 
r2* (cos (t2) *sin (a2) -sin (t2) *cos (a2) ) . . 
r3* (cos (t3) *sin (a3) -sin (t3) *cos  (a3) ) ]  ; 


B  = 


[-1  0  0; 

0  -1  -1; 

0  -.192  0]  ; 


Drag  =  [0.05  0  0;0  0.05  0;0  0  0.15]; 

tolerance  =  0.5; 
a_tol  =  10; 
flag  =  0; 

Q _ 

o - 

%set  up  com  link 

O _ 

O - 


format  compact 
plotimage  =  1; 
communication  =  1; 
if  (communication  ==1) 

s3  =  serial (' C0M1 baudrate 1 9200 ) ; 
f open ( s3 ) 

end 

xd  pix  =  xd*x  conversionA-l ; 
yd  pix  =  yd*y  conversionA-l ; 

Q _ 

o - 


initialize  camera 
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%  I  think  camera  needs  to  be  plugged  in  and  creative  cam  software  is  off 
before  you  start  matlab 
%  set  up  video  object 
vidobj  =  videoinput (' winvideo ')  ; 

%  now  you  must  trigger  it  to  log  data 
triggerconf ig (vidobj , 'manual ' ) ; 

%  only  log  a  single  frame 

set (vidobj ,  ' f ramespertrigger '  ,  1); 

%  lets  you  trigger  it  as  many  times  as  you  want 
set (vidobj ,  ' triggerrepeat ' ,  inf) 

pose_hist  =  []; 

%start  but  don't  trigger  (log) 
start (vidobj ) 

%  wait  for  warm  up 
pause  ( 1 ) 

frame_times  =  []; 

i  =  1; 

tic; 

while (flag==0) 


o _ 

O - 

%GETS  VIDEO  FEED  AND  DETERMINES  THE  VEHICLES  POSITION  AND  ATTITUDE 

O _ 

o - 

%log  a  frame 
trigger (vidobj ) ; 

%  load  it  into  memory  with  time  stamp 
[frame  time]  =  getdata (vidobj ) ; 

%  just  keeping  track  of  how  many  frames  per  sec  we  are  getting 
frame  times  =  [frame  times;  time]; 

if (1 — 1) 

%  define  robots  are  a  certain  region  of  color  space 

green  =  frame(:,:,l)  >230  &  f rame ( : , : , 2 ) <1 60  &  frame ( : , : , 3) <150; 

%actually  this  is  red 

red  =  frame (:,:,1)  <  130  &  frame ( : , : , 2 ) >220  &  f rame ( : , : , 3 ) >50 ;  %actually 
this  is  green 

%  looking  for  8  connected  comopnents 
R  =bwlabel ( red, 8 ) ; 

Y  =bwlabel (green,  8 )  ; 

%  using  the  labeled  matrix  will  use  Dunbar's  optimized  property  finder 
%  this  is  the  built  in  version 
s  =  regionprops (R,  'centroid',  'area'); 
t  =  regionprops (Y,  'centroid',  'area'); 

%for  red  led's 

centroids_L  =  cat(l,  s. Centroid); 
area_L  =  cat(l,  s.Area); 
robots_L  =  find (area_L>50) ; 

%for  white  led's 

centroids_R  =  cat(l,  t. Centroid); 
area  R  =  cat(l,  t.Area); 


robots_R  =  find (area_R>50) ; 

%  robots  have  to  be  bigger  than  some  #  of  pixels  to  eliminate 
%  spurrious  results 

%can  display  to  screen  at  cost  of  computation  time ...  actually  this 
%barely  impacts  it 
end 

if  (plotimage  ==  1) 

if  (plotimage  ==  1) 
image sc (frame) 
figure ( 1 ) 
hold  on 


end 

if  -isempty (robots_R) 
if  -isempty ( robots_L) 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


PLOTS  THE  POSITION  AND  ORIENTATION  ON  THE  IMAGE%%% 


S^9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'S^9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'S^9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


if  (plotimage  ==  1) 


plot (centroids_R ( robots_R, 1 ) ,  centroids_R (robots_R, 2 ) ,  1 k*  1  )  ; 

plot (xd_pix,  4  8  0-yd_pix,  1 r+ ' ) ; 

plot ( [xd_pix+50*cos ( sid)  xd_pix-50*cos (sid) ] ,  [480— 

(yd_pix+50*sin (sid) )  480- (yd_pix-50*sin (sid) ) ] ) ; 


plot (centroids_L (robots_L, 1) ,  centroids_L (robots^L, 2 ) ,  ' c* ' )  ; 

plot ( [centroids_R ( robots_R ( 1 ) , 1 )  centroids_L ( robots_L ( 1 ) , 1 ) ] . 
,  [centroids  R(robots  R(l),2)  centroids  L(robots  L(l),2)] 


plot (1/2* (centroids_R ( robots_R ( 1 ) ,  1 )  - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 ) , 1 ) ... 

,  1/2*  (centroids_R  ( robots_R  ( 1 )  ,2)  - 
centroids_L ( robots_L ( 1 ) ,2) ) +centroids_L ( robots_L ( 1 ) , 2 ) ,  ' r* ' ) ; 

end 

if  (i<runtime) 

M(i)  =  getframe; 

end 

%from  green  to  red  LED  measured  from  normal  x-axis  on  cartesian 
%coordinates 

si  =  atan2 (centroids_R (robots_R ( 1 ) ,2) - (centroids_L (robots_L ( 1 ) , 2 ) ) . . . 
, (centroids_L ( robots_L ( 1 ) , 1 ) ) -centroids_R (robots_R (1) , 1) ) *180/pi; 


%makes  sure  angle  is  from  0  to  360  degrees 
if  (si  <  0) 

si  =  si  +  360; 

end 

%converts  to  radians 
si  =  si*pi/180; 


error  si 


sid- si ; 


if  (error  si  >  pi) 

error  si  =  error  si- (360*pi/180) ; 

end 

if  (error  si  <  -pi) 

error_si  =  error  si+ (360*pi/180) ; 

end 


pos  =  [  1 /2* (centroids_R (robots_R ( 1 )  ,  1 ) - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 ) , 1 ) 

,  1/2* (centroids_R (robots_R ( 1 ) , 2 ) - 
centroids_L ( robots_L ( 1 ) ,2) ) +centroids_L ( robots_L ( 1 ) , 2 ) ] ; 
x_pose  =  pos(l); 
y_pose  =  480-pos(2); 

%Logs  x,y,si,and  time 
x_log (i) =x_pose; 
y_log (i) =y_pose; 
si_log(i)  =  si; 
timer  =  toe; 
time  log(i)  =  timer; 

end 

end 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9- 

oooooooooooooooooooooo 


DEFINE  MEMBER  MATRICIES 


9'9'9'9-9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9' 

ooooooooooooooooooooooooooo 


9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Rot  =  [cos  (si)  -sin(si)  0  ; 

sin ( si )  cos  ( si )  0  ; 

0  0  1]; 

%converts  pixels  to  m 
x  =  x  pose*x  conversion; 
y  =  y  pose*y  conversion; 


%  vectors 

error  =  [xd-x  yd-y  error  si] '; 
Pd  =  [xd  yd  sid] ' ; 

PI  =  [x  y  si ]  '  ; 

o _ 

O - 


CALCULATE  ERROR  DERIVATIVE 
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%  derivatives  using  rise  over  run  technique 
if  (i==l) 

error_dot  =  [0  0  0] 

Pd_dot  =  [0  0  0]  '  ; 

Pl__dot  =  [0  0  0]  '  ; 

Pd_dot_dot  =  [0  0  0]  '; 

else 

error  dot  =  (error-error  age) / (timer-time  age) ; 
Pd_dot  =  (Pd-Pd_age) / (timer-time_age) ; 

Pl_dot  =  (Pl-Pl_age) / (timer-time_age) ; 

Pd_dot_dot  =  (Pd_dot-Pd_dot_age) / (timer-time_age) ; 

end 

%error  signal  r 

r  error  =  error  dot  +  k  alpha  *  error; 

%//================================================= 

%//implement  full  controller 

O, 

o  •  *  • 


U  =  inv (Rot*inv (Mass) *B) * ( (Pd  dot  dot+k  alpha*error  dot)+(Kr*r  error)... 
+(P1  dot(3)*sl*Pl  dot) + (Rot*inv (Mass) *Drag*Rot ' *P1  dot)); 

%Allocate  thrusts 
Ua  =  U  ( 1 )  ; 

Ub  =  U  ( 2  )  ; 

Uc  =  U  ( 3 )  ; 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- &- A  P T?  THT?  T  Df  ATT  PM  TT ?  P  M  Q  9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 
OOOOOOOOOOOOOOOOOOOOO  OiAoJli  ±  nj_i  LUt/nl  1UIN  ±  Hir\lvJ.O  oooooooooooooooooooooooooooo 

9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9-9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

Pd_age  =  Pd; 

Pl_age  =  PI; 

Pd_dot_age  =  Pd_dot; 
error  age  =  error; 
time  age  =  timer; 


9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'QT?T\Tri  THDTTQTQ  TH  THTT  RD  A  T  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9- 
ooooooooooooooooooooooOlliIMU  ini\UiJ  I  O  -Lv/  inJj  JD  wiA  ±  oooooooooooooooooooooooooo 

9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9-9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

if  (communication  ==1) 


my_stringl  =  sprintf ( ' a%5 . 3f \r ' ,  Ua) ; 
fprintf(s3,  my  stringl); 

my_string2  =  sprintf (' b%5 . 3f\r ' ,  Ub) ; 
fprintf(s3,  my  string2); 

my_string3  =  sprintf (' c%5 . 3f\r ' ,  Uc) ; 
fprintf(s3,  my_string3); 


end 
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S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


CALCULATE  AND  LOG  INDIVIDUAL  THRUSTS 


0.000 
o  o  o  o 


9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Ul  =  0 . 5* (Ua+sqrt (Ua*Ua+gamO*gamO ) ) ; 

U4  =  abs (0 . 5* (-Ua+sqrt (Ua*Ua+gamO*gamO) )) ; 

U2  =  0 . 5* (Ub+sqrt (Ub*Ub+gamO*gamO) ) ; 

U5  =  abs (0 . 5* (-Ub+sqrt (Ub*Ub+gamO*gamO ) )) ; 

U3  =  0 . 5* (Uc+sqrt (Uc*Uc+gamO*gamO) ) ; 

U6  =  abs (0 . 5* (-Uc+sqrt (Uc*Uc+gamO*gamO ) )) ; 

if  (Ul>2 . 4 ) 

Ul  =  2.4; 

end 

if  (U4>2 . 4 ) 

U4  =  2.4; 

end 

if  (U2>1 . 6) 

U2  =  1.6; 

end 

if  (U3>1 . 6) 

U3  =  1.6; 

end 

if  (U5>1 . 6) 

U5  =  1.6; 

end 

if  (U6>1 . 6) 

U  6  =  1.6; 

end 

Ul_log(i)  =  Ul; 

U4_log(i)  =  U4 ; 

U2_log(i)  =  U2; 

U5_log(i)  =  U5; 

U3_log(i)  =  U3; 

U6_log(i)  =  U6; 


end 


%if  ( (x__des-tolerance)  <  x_pose  &&  (x_des+tolerance)  >  x_pose 
tolerance)  <  y_pose  &&... 

%  (y_des+tolerance)  >  y_pose  &&  (angle_des  -  a_tol)  <  si 

(angle_des+a_tol ) >  si*180/pi) 
if (i>runtime) 
flag  =  1; 

end 
i=i  +  l 


S-S-S-S-S-S-S-S-S-S- 

oooooooooo 

S-S-S-S-S-S-S-S-S-S- 

oooooooooo 


&  &  ( y_de  s - 
*180/pi  && 


end 


9'9'9'9-9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooo 


RESET  VARIABLES  AND  PROPERLY  TERMINATE  PROGRAM 


oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


flag  =  0; 

avg  time  =  mean (frame  times (3: end)  - 
%close  all 

%plot  (x_m_p*pose_hist  (  :  ,  1)  ,  y__m_p*po 
%grid  on; 


frame  times (2 : end-1 ) ) 
se  hist ( : , 2 ) , 1  *-  '  ) 


%  This  is  frame  time 

%plot (frame  times(3:end)  -  frame  times (2 : end-1 ) ) 


delete (vidobj ) ; 

if  (communication  ==1) 

fclose ( s3 ) ; 

delete ( s3 ) ; 

end 

16.2  Controller  II  Code 


16.2.1  Controller  II  Simulation  Code 


SwarmDynKin: 


function  [ sys , xO , str , ts ]  =  SwarmDynKin (t, x, u, flag) 


switch  flag, 

O, _ 

Q - 

%  Initialization 

O, _ 

Q - 

case  0, 

[sys,xO,str,ts] =mdl Initialize Sizes; 


%  Derivatives 


case  1, 

sys=mdlDerivatives (t, x, u) ; 


%  Update 


case  2, 

sys=mdlUpdate (t, x, u) ; 


%  Outputs 


case  3, 

sys=mdlOutputs (t, x, u) ; 


GetTimeOfNextVarHit 
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g, _ 

o - 

case  4, 

sys=mdlGetTimeOfNextVarHit  (t, x, u)  ; 


g, _ 

Q - 

%  Terminate 

O, _ 

Q - 

case  9, 

sys=mdlTerminate (t, x, u) ; 


g, _ 

Q - 

%  Unexpected  flags 

g, _ 

o - 

otherwise 

error ([' Unhandled  flag  =  ' , num2str (flag) ] ) ; 

end 

Q,  Q, _ 

O  o - 

%  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-function 

g, _ 

o - 

function  [sys,xO,str,ts] =mdl Initialize Sizes 

%  Call  simsizes  for  a  sizes  structure,  fill  it  in  and  convert  it  to  a  sizes 
%  array. 

%  Note  that  in  this  example,  the  values  are  hard  coded.  This  is  not  a 
%  recommended  practice  as  the  characteristics  of  the  block  are  typically 
%  defined  by  the  S-function  parameters. 

sizes  =  simsizes; 

sizes . NumContStates  =  9; 
sizes . NumDiscStates  =  0; 
sizes .NumOutputs  =  15; 

sizes .Numlnputs  =  0; 

sizes . DirFeedthrough  =  1; 

sizes . NumSampleTimes  =1;  %  at  least  one  sample  time  is  needed 


sys  =  simsizes (sizes); 


xO  =  [5.10  1.53  (285.0) *pi/180.0  0.0  0.0  0.0  0.05  0.05  0.15]'; 

%  str  is  always  an  empty  matrix 
str  =  [ ] ; 

%  Initialize  the  array  of  sample  times 
ts  =[00]; 

%  end  mdllnitializeSizes 


g,  g, 
o  o 


mdl Derivatives 


%  Return  the  derivatives  for  the  continuous  states. 

O, _ 

O - 

function  sys=mdlDerivatives (t, x, u) 

%  Exchange  of  variables 
Px  =  x  ( 1 ,  1 )  ; 

Py  =  x ( 2 , 1 )  ; 
psi  =  x (3, 1)  ; 

Vx  =  x ( 4 , 1 )  ; 

Vy  =  x  ( 5 , 1 )  ; 

Vpsi  =  x ( 6, 1 )  ; 

D1  =  x (7, 1)  ; 

D2  =  x  (  8  ,  1 )  ; 

D3  =  x  (  9 ,  1 )  ; 
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V  =  [Vx  Vy  Vpsi] 

P  =  [Px  Py  psi] 1 ; 
theta  hat  =  [D1  D2  D3] 

[theta_hat_dot, dP, dV, U, e]  =  DynamicsControl (theta_hat, P, V) ; 
sys  =  [  dP;  dV;  theta_hat_dot] ; 

%  end  mdlDerivatives 


%  mdlUpdate 

%  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
%  requirements. 

Q, 

O 


function  sys=mdlUpdate (t, x, u) 
sys  =  [ ] ; 

%  end  mdlUpdate 


%  mdlOutputs 

%  Return  the  block  outputs. 

o, 

o 


function  sys=mdlOutputs (t, x, u) 

%  Exchange  of  variables 
Px  =  x  ( 1 ,  1 )  ; 

Py  =  x ( 2 , 1 )  ; 
psi  =  x  (3, 1)  ; 


Vx  =  x  ( 4 , 1 )  ; 
Vy  =  x  ( 5 ,  1 )  ; 
Vpsi  =  x  (  6, 1)  ; 


D1  =  x (7, 1)  ; 
D2  =  x  (  8  ,  1 )  ; 
D3  =  x  (  9 ,  1 )  ; 
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V  =  [Vx  Vy  Vpsi] ' ; 

P  =  [ Px  Py  psi ] ' ; 
theta  hat  =  [D1  D2  D3] 

[theta_hat_dot, dP, dV, Us, e]  =  DynamicsControl (theta_hat, P, V) ; 

O, 

o 


%  Output  Vector 

o, 

o 


sys  =  [ P ' , Us ' , e ' , theta_hat ' ] ; 
%  end  mdlOutputs 


%  mdlGetTimeOfNextVarHit 

%  Return  the  time  of  the  next  hit  for  this  block.  Note  that  the  result  is 
%  absolute  time.  Note  that  this  function  is  only  used  when  you  specify  a 
%  variable  discrete-time  sample  time  [-2  0]  in  the  sample  time  array  in 
%  mdllnitializeSizes .mdl 

O, 

o 


function  sys=mdlGetTimeOfNextVarHit (t, x, u) 

sampleTime  =1;  %  Example,  set  the  next  hit  to  be  one  second  later, 

sys  =  t  +  sampleTime; 

%  end  mdlGetTimeOfNextVarHit 


%  mdlTerminate 

%  Perform  any  end  of  simulation  tasks. 

O, 

o 


function  sys=mdlTerminate (t, x, u) 
sys  =  [ ] ; 

%  end  mdlTerminate 


DynamicsControl: 

function  [theta_hat_dot, dP, dV, U, e]  =  DynamicsControl (theta_hat, P, V) 

O, _ 

O - 

%  System  Parameters  and  Matrix  Definitions 

O, _ 

O - 

%  Tugboat  locations 
rl  =  0.6; 

alphal  =  (180.0) *pi/180.0; 
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thetal  =  (0 . 0) *pi/180 . 0; 

r2  =0.27; 

alpha2  =  (270 . 0) *pi/180 . 0; 

theta2  =  (44 . 72) *pi/180 . 0; 

r3  =  0.19; 

alpha3  =  (270 . 0) *pi/180; 

theta3  =  (90 . 0) *pi/180 . 0; 

r4  =  rl; 

alpha4  =  ( 0 . 0 ) *pi/180 . 0 ; 
theta4  =  ( 180 . 0 ) *pi/180 . 0 ; 

r5  =  r2  ; 

alpha5  =  (90 . 0) *pi/180 . 0; 
theta5  =  (360 . 0) *pi/180 . 0-theta2; 

r6  =  r3; 

alpha6  =  (90 . 0) *pi/180 . 0; 
theta6  =  (270 . 0) *pi/180 . 0; 

dl  =  0.05; 
d2  =  0.05; 
d3  =  0.15; 
gaml  =  1 ; 
gam2  =  1 ; 
gam3  =  1 ; 


%  Thrust  matrix 

B1  =  [cos(alphal)  cos(alpha2)  cos(alpha3)  cos(alpha4)  cos(alpha5) 
cos (alpha6) ] ; 

B2  =  [sin(alphal)  sin(alpha2)  sin(alpha3)  sin(alpha4)  sin(alpha5) 
sin (alpha6) ] ; 

B3  =  [rl*sin (alphal-thetal)  r2*sin (alpha2-theta2 )  r3*sin (alpha3-theta3)  ... 
r4*sin (alpha4-theta4 )  r5*sin (alpha5-theta5)  r6*sin (alpha6-theta6) ] ; 

B  =  [Bl; 

B2  ; 

B3  ]  ; 

%  For  the  above  configuration 
Bs  =  [-1  0  0; 

0  -1  -1; 

0  -r2*cos (theta2)  0  ]; 

%  Mass  matrix 
m  =  15.5129;  %  (kg) 

Iz  =  1.5849;  %  (kgmA2) 

M  =  [m  0  0; 

0  m  0  ; 

0  0  Iz]  ; 
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%%  Control 


Px 

=  p  ( l ,  l )  ; 

Py 

=  P  ( 2 , 1 )  ; 

psi 

=  P  (3, 1)  ; 

%  Rotation  Matrix 

R  = 

[cos (psi) 

-sin (psi ) 

0; 

sin (psi ) 

cos (psi ) 

0; 

0 

0 

1]  ; 

Pdot 

=  R*V; 

%  Desired  Trajectories 

Pd  =  [2  2  (90.0) *pi/180 . 0]  '  ; 

PdDot  =  [0.0  0.0  0.0]  '  ; 

PdDDot  =  [0.0  0.0  0.0]'; 

%  Control  gains 
gammaO  =  0.0; 

%Kp  =  0.05; 

%  Kr  =  0.5; 

%  alpha  =  1.0; 

%  Kr  =  [.600; 

%  0.6  0; 

%  00.8]; 

Kr  =  [2.0  0  0; 

0  5.0  0; 

0  0  4.0]; 

alpha  =  [ . 3  0  0 ; 

0  .3  0; 

0  0  .35] ; 

gamma  =  [garni  0  0; 

0  gam2  0 
0  0  gam3] ; 

D  =  [dl  0  0; 

0  d2  0; 

0  0  d3] ; 

Y  =  [cos (psi) *Pdot (1) +sin (psi) *Pdot (2)  0  0; 

0  cos (psi) *Pdot (2) -sin (psi) *Pdot (1)  0 
0  0  Pdot ( 3 ) ] ; 

%  Error  signals 
e  =  Pd-P; 
eDot  =  PdDot-Pdot; 
r  =  eDot+alpha*e; 

theta  hat  dot  =  (r ' *R*inv (M) *Y*gamma) '; 

S  =  [0  10; 


-1  0  0; 

0  0  0  ]  ; 
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Us 

inv (R*inv (M) *Bs) * ( PdDDot+alpha*eDot+P (3,1) *S*Pdot+Kr*r+R*inv (M) *Y*theta  hat) ; 
%  Us  =  inv (R*inv (M) *Bs) * (Kp*e) ; 

ul4  =  Us  (1, 1)  ; 
u2  5  =  Us  (2 , 1)  ; 
u36  =  Us (3, 1)  ; 

ul  =  0.5* (ul4  +sqrt (ul4A2+gammaOA2) ) ; 
u4  =  0 . 5*  (~ul4  +  sqrt (ul4A2+gammaOA2) ) ; 

u2  =  0.5* (u25  +sqrt (u25 A2+gamma0 A2 ) ) ; 
u5  =  0 . 5* (~u25+sqrt (u25A2+gammaOA2) ) ; 

u3  =  0.5* (u36  +sqrt (u36A2+gammaO A2 ) ) ; 
u6  =  0 . 5* (~u36+sqrt (u36A2+gammaOA2) ) ; 

if  (ul>2 . 4 ) 
ul  =  2.4; 

end 

if  (u4>2 . 4 ) 
u4  =  2.4; 

end 

if  (u2>l . 6) 

u2  =  1.6; 

end 

if  (u3>l . 6) 
u3  =  1.6; 

end 

if  (u5>l . 6) 
u5  =  1.6; 

end 

if  (u6>1.6) 
u  6  =  1.6; 

end 


U  =  [ul  u2  u3  u4  u5  u6] ' ; 

%%  Kinematics 
dP  =  R*V; 

%%  Swarm/Barge  System  Dynamics 
dV  =  inv (M) * ( -D*V+B*U) ; 

return; 


plot_angle : 


close  all 
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length  =  151; 
plot ( Px, Py, ' -b ' ) 
axis (  [ 0  8  0  4.4]); 
hold  on 

for  i  =  1:3: length 

plot( [Px(i)+.2*cos(psi(i) )  Px(i)-.2*cos(psi(i) ) ] ,  [Py(i)+0.2*sin(psi(i) ) 

Py(i)-0.2*sin(psi(i))],  ' —  k ' )  ; 

plot(Px(i)+.2*cos(psi(i) ) ,Py(i)+0.2*sin(psi(i) ) , ' *g ' ) ; 
plot(Px(i)-.2*cos(psi(i) ) , Py(i)-0.2*sin(psi(i) ) , ' *r ' ) 
end 

title (' Simulated  Position  and  Orientation  for  IC-1  using  adaptive  D 
controller ' ) 

xlabel('X  position  (m) ') 
ylabel('Y  position  (m) ') 

figure (2 ) 
subplot (2,2,1)  ; 

plot (Time, theta_hat ( :  ,  1 )  ,  '  -r  '  ) 
title (' Parameter :  D1 '  ) 
xlabel('time  (s)  ') 
ylabel (' Parameter  Value') 
subplot (2,2,2)  ; 

plot (Time, theta_hat  (  :  , 2 ) ,  '  -b  '  ) 
title (' Parameter :  D2 ' ) 
xlabel('time  (s) ') 
ylabel (' Parameter  Value') 
subplot (2,2,3) ; 

plot (Time, theta_hat ( :  ,  3)  ,  ' -c '  ) 
title (' Parameter :  D3  '  ) 
xlabel('time  (s)  ') 
ylabel (' Parameter  Value') 

16.2.2  Controller  II  Experimental  Code 

Adaptivedragexp : 

clear  M 
clear  all 

%desired  location  and  orientation  in  pixels 
x  des=input (' Input  the  desired  X  position'); 
y_des=input (' Input  the  desired  Y  position'); 
angle  des=input (' Input  the  desired  angle'); 
runtime  =  input (' Input  the  run  time'); 

O _ 

O - 

%initialize  variables  and  vessel  parameters 

o _ 

O - 

x_log  =  [];  y_log  =  [];  si_log  =  [];  Ul_log  =  [];  U2_log  =  []; 

U3_log  =  [];  U4_log  =  [];  U5_log  =  [];  U6_log  =  [];  time_log  =  []; 

x  conversion  =  7.927/640; 
y  conversion  =  4.4/480; 

%  Kr  =  [ . 2  0  0 ; 

%  0  .5  0; 

%  00.6]; 

%  k_alpha  =  [.3  0  0; 


0  .3  0; 

0  0  .35]  ; 
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Kr  =  [ . 2  0  0 ; 

0  .5  0; 

0  0  .4]; 

k_alpha  =  [.3  0  0; 

0  .3  0; 

0  0  .35]  ; 
al  =  180*pi/180; 
a2  =  270*pi/180; 
a3  =  270*pi/180; 

j  =  1.5849;  %using  moment  of  inertia  formula  for  a  cuboid  from 

%http : / /www . diracdelta .co.uk/ science/ source/m/ o /moment s%2 Oof %20 inertia/ source 

.  html 

m  =  15.5129; 
rl  =  0.6; 
r2  =  0.27; 
r3  =  0.19; 
si  =  0  ; 

tl  =  0*pi/180; 
t2  =  44 . 72*pi/180; 
t3  =  90*pi/180; 
xd  =  x_de  s ; 
yd  =  y_de  s ; 

sid  =  angle_des*pi/180; 


gamO 

=  0; 

error 

age 

= 

0; 

timer 

=  0; 

time 

age  = 

= 

0; 

si  = 

[  o 

1 

0; 

-1 

0 

0; 

0 

0 

0]  ; 

Mass 

=  [m 

0 

0; 

0 

m 

0; 

0 

0 

j  ] ; 

B  =  [cos(al)  cos(a2)  cos(a3); 
sin(al)  sin(a2)  sin(a3); 
rl* (cos (tl)  *sin (al) -sin (tl) *cos  (al) )  .  .  . 
r2* (cos (t2) *sin (a2) -sin (t2) *cos (a2) ) . . . 
r3* (cos (t3) *sin (a3) -sin (t3) *cos  (a3) ) ]  ; 

dl  =  0.1; 
d2  =  0.1; 
d3  =  0.1; 


%theta  =  [dl  d2  d3] ' ; 

gamma  =[100 
0  10 
0  0  1]; 
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flag  =  0; 


O _ 

Q - 

%set  up  com 

o _ 

0 - 


link 


format  compact 
plotimage  =  1; 
communication  =  1; 
if  (communication  ==1) 

s3  =  serial (' COM1 ',' baudrate ', 1 9200 ) ; 
f open  ( s3 ) 

end 

xd  pix  =  xd*x  conversionA-l ; 
yd  pix  =  yd*y  conversionA-l ; 

O _ 

O - 

%initialize  camera 

O _ 

O - 

%  I  think  camera  needs  to  be  plugged  in  and  creative  cam  software  is  off 
before  you  start  matlab 
%  set  up  video  object 
vidobj  =  videoinput (' winvideo ')  ; 

%  now  you  must  trigger  it  to  log  data 
triggerconf ig (vidobj , 'manual ' ) ; 

%  only  log  a  single  frame 

set (vidobj ,  ' f ramespertrigger '  ,  1); 

%  lets  you  trigger  it  as  many  times  as  you  want 
set (vidobj ,  ' triggerrepeat ' ,  inf) 

pose_hist  =  []; 

%start  but  don't  trigger  (log) 
start (vidobj ) 

%  wait  for  warm  up 
pause  ( 1 ) 

frame_times  =  []; 

i  =  1; 

tic; 

while (flag==0) 


o _ 

O - 

%GETS  VIDEO  FEED  AND  DETERMINES  THE  VEHICLES  POSITION  AND  ATTITUDE 

O _ 

o - 

%log  a  frame 
trigger (vidobj ) ; 

%  load  it  into  memory  with  time  stamp 
[frame  time]  =  getdata (vidobj )  ; 

%  just  keeping  track  of  how  many  frames  per  sec  we  are  getting 
frame  times  =  [frame  times;  time]; 

if (1 — 1) 

%  define  robots  are  a  certain  region  of  color  space 
green  =  frame(:,:,l)  >230  &  f rame ( : , : , 2 ) <1 60  &  frame ( : , : , 3) <150; 
%actually  this  is  red 
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red  =  frame <  130  &  frame ( : , : , 2 ) >220  &  f rame ( : , : , 3 ) >50 ;  %actually 
this  is  green 

%  looking  for  8  connected  comopnents 
R  =bwlabel ( red, 8 ) ; 

Y  =bwlabel (green,  8 )  ; 

%  using  the  labeled  matrix  will  use  Dunbar's  optimized  property  finder 
%  this  is  the  built  in  version 
s  =  regionprops (R,  'centroid',  'area'); 
t  =  regionprops (Y,  'centroid',  'area'); 

%for  red  led's 

centroids_L  =  cat(l,  s. Centroid); 
area_L  =  cat(l,  s.Area); 
robots_L  =  find (area_L>50) ; 

%for  white  led's 

centroids_R  =  cat(l,  t. Centroid); 
area_R  =  cat(l,  t.Area); 
robots_R  =  find (area_R>50) ; 

%  robots  have  to  be  bigger  than  some  #  of  pixels  to  eliminate 
%  spurrious  results 

%can  display  to  screen  at  cost  of  computation  time ...  actually  this 
%barely  impacts  it 
end 

if  (plotimage  ==  1) 

if  (plotimage  ==  1) 
image sc (frame) 
figure ( 1 ) 
hold  on 


end 

if  -isempty ( robots_R) 
if  -isempty (robots_L) 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


PLOTS  THE  POSITION  AND  ORIENTATION  ON  THE  IMAGE%%% 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


if  (plotimage  ==  1) 

plot (centroids_R ( robots_R, 1 ) ,  centroids_R (robots_R, 2 ) ,  ' k*  '  )  ; 


plot (xd_pix,  4  8  0-yd_pix,  'r+'); 

plot ( [xd_pix+50*cos ( sid)  xd_pix-50*cos ( sid) ] ,  [480- 

(yd_pix+50*sin (sid) )  480- (yd_pix-50*sin (sid) )]); 

plot (centroids_L (robots_L, 1 ) ,  centroids_L (robots_L, 2 ) ,  '  c* ' ) ; 


plot ( [centroids_R ( robots_R ( 1 ) , 1 )  centroids_L ( robots_L ( 1 ) , 1 ) ] . . . 
,  [centroids  R(robots  R(l),2)  centroids  L(robots  L(l) ,2) ] , 


plot (1/2* (centroids_R ( robots_R ( 1 ) , 1 ) - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 ) , 1 ) ... 

,  1/2* (centroids_R ( robots_R ( 1 )  ,  2 ) - 
centroids_L ( robots_L ( 1 ) , 2 ) ) +centroids_L ( robots_L ( 1 ) , 2 ) ,  ' r* ' ) ; 

end 

if  (i<runtime) 


o\°  o\°  o\° 


M(i)  =  getframe; 
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end 

%from  green  to  red  LED  measured  from  normal  x-axis  on  cartesian 
%coordinates 

si  =  atan2 (centroids__R (robots_R ( 1 ) , 2 ) - (centroids_L (robots_L ( 1 ) , 2 ) ) 

, (centroids_L (robots_L ( 1 ) , 1 ) ) -centroids_R ( robot s_R (1) , 1) ) *180/pi; 

%makes  sure  angle  is  from  0  to  360  degrees 
if  (si  <  0) 

si  =  si  +  360; 

end 

%converts  to  radians 
si  =  si*pi/180; 

error  si  =  sid-si; 
if  (error_si  >  pi) 

error_si  =  error  si- (360*pi/180) ; 

end 

if  (error  si  <  -pi) 

error  si  =  error  si+ (360*pi/180) ; 

end 


pos  =  [  1 /2* (centroids_R ( robots_R ( 1 )  ,  1 ) - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 ) , 1 ) 

,  1/2* (centroids_R (robots_R ( 1 )  ,  2 ) - 
centroids_L ( robots_L ( 1 ) ,2) ) +centroids_L ( robots_L ( 1 ) , 2 ) ] ; 
x_pose  =  pos(l); 
y_pose  =  480-pos(2); 

%Logs  x,y,si,and  time 
x_log (i) =x_pose; 
y_log (i) =y_pose; 
si_log(i)  =  si; 
timer  =  toe; 
time  log(i)  =  timer; 

end 

end 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


DEFINE  MEMBER  MATRICIES 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

ooooooooooooooooooooooooooo 


9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Rot 


[cos (si)  -sin(si)  0  ; 
sin ( si )  cos ( si )  0  ; 


0 


0  1]  ; 
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%converts  pixels  to  m 
x  =  x  pose*x  conversion; 
y  =  y  pose*y  conversion; 


%  vectors 

error  =  [xd-x  yd-y  error  si] '; 
Pd  =  [xd  yd  sid] ' ; 

PI  =  [x  y  si ]  '  ; 

O _ 

O - 

%CALCULATE  ERROR  DERIVATIVE 

O _ 

O - 


%  derivatives  using  rise  over  run  technique 
if  (i==l) 

error_dot  =  [0  0  0]  '; 

Pd_dot  =  [0  0  0]  '  ; 

Pl_dot  =  [0  0  0]  '  ; 

Pd_dot_dot  =  [0  0  0]  '; 

else 

error  dot  =  (error-error  age) / (timer-time  age) ; 
Pd_dot  =  (Pd-Pd_age) / (timer-time_age) ; 

Pl_dot  =  (Pl-Pl_age) / (timer-time_age) ; 

Pd_dot_dot  =  (Pd_dot-Pd_dot_age) / (timer-time_age) ; 

end 

Y_ad  =  [cos (si) *Pl_dot (1) +sin (si) *Pl_dot (2)  0  0; 

0  cos (si) *Pl_dot (2) -sin (si) *Pl_dot (1)  0 
0  0  Pl_dot (3 ) ] ; 

%error  signal  r 

r  error  =  error  dot  +  k  alpha  *  error; 

%%%%%%%%need  to  set  up  integrator 

theta  hat  dot  =  (r  error ' *Rot*inv (Mass) *Y  ad*gamma) '; 

if (i — 1) 

theta_hat  =  [dl  d2  d3] '; 
else 

theta  hat  =  theta  hat+ ( (timer-time_age) * (theta  hat  dot  + 

theta_hat_dot_age) /2) ; 

end 

theta_hat_log ( 1 , i )  =  theta_hat(l); 
theta_hat_log (2, i)  =  theta_hat(2); 
theta_hat_log ( 3 , i )  =  theta_hat ( 3 ) ; 

%//================================================= 

%//implement  full  controller 

O, 

o  •  *  • 


U  =  inv (Rot*inv (Mass) *B) * ( (Pd  dot  dot+k  alpha*error  dot)... 


109 


+(P1  dot(3)*sl*Pl  dot)+(Kr*r  error) + (Rot*inv (Mass) *Y_ad*theta  hat)); 

%U  =  inv (Rot*inv (Mass) *B) * ( (Kr*error) )  ; 

%Allocate  thrusts 
Ua  =  U  ( 1 )  ; 

Ub  =  U  ( 2  )  ; 

Uc  =  U  ( 3 )  ; 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9'9'9'9'2'9'9'9'9'9'2'9'2'9'9'9'9'9'2'9'2'9'7\PTh1  THTh1  T  nfUTTDM  Tin1  P M Q  2- 9- 9- 9- 2- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 9- 2- 9- 9- 9- 9- 9- 9- 9- 
ooooooooooooooooooooo  orlolli  ±  nj_i  J_i  ±  _L  wi\i  ±J_jr\lv10  oooooooooooooooooooooooooooo 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

Pd_age  =  Pd; 

Pipage  =  PI; 

Pd_dot_age  =  Pd_dot; 
error  age  =  error; 
time  age  =  timer; 

theta_hat_dot_age  =  theta_hat_dot; 


9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'Q171\Tri  THDT1QTQ  TH  T  WT7  RD  A  T  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  2-  9-  9-  9-  9-  9- 
OOOOOOOOOOOOOOOOOOOOO  oOJ-iiMU  injAUijiiJ  -L  w  .L  n.J_j  DUni  oooooooooooooooooooooooooo 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

if  (communication  ==1) 


my_stringl  =  sprintf ( ' a%5 . 3f \r ' ,  Ua)  ; 
fprintf(s3,  my  stringl); 

my_string2  =  sprintf (' b%5 . 3f\r ' ,  Ub)  ; 
fprintf(s3,  my  string2); 

my_string3  =  sprintf (' c%5 . 3f\r ' ,  Uc)  ; 
fprintf(s3,  my_string3); 


end 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

ooooooooooooooooooooo 


%CALCULATE  AND  LOG  INDIVIDUAL  THRUSTS 


S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Ul  =  0 . 5* (Ua+sqrt (Ua*Ua+gamO*gamO) ) ; 

U4  =  abs ( 0 . 5* ( -Ua+sqrt (Ua*Ua+gamO*gamO ))) ; 

U2  =  0 . 5* (Ub+sqrt (Ub*Ub+gamO*gamO) ) ; 

U5  =  abs ( 0 . 5* ( -Ub+sqrt (Ub*Ub+gamO*gamO ))) ; 

U3  =  0 . 5* (Uc+sqrt (Uc*Uc+gamO*gamO) ) ; 

U6  =  abs (0 . 5* (-Uc+sqrt (Uc*Uc+gamO*gamO ) )) ; 

if  (Ul>2 . 4 ) 

Ul  =  2.4; 

end 

if  (U4>2 . 4 ) 

U4  =  2.4; 


end 
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if 

(U2>1 . 

■  6) 

U2  = 

1 . 

6; 

end 

if 

(U3>1 . 

■  6) 

U3  = 

1 . 

6; 

end 

if 

(U5>1 . 

■  6) 

U5  = 

1 . 

6; 

end 

if 

(U6>1 . 

,6) 

U6  = 

1 . 

6; 

end 

U1 

log (i) 

= 

Ul; 

U4 

log (i) 

= 

U4  ; 

U2 

log (i) 

= 

U2 ; 

1 

LO 

D 

log (i) 

= 

U5  ; 

a 

OJ 

l 

log (i) 

= 

U3 ; 

a 

1 

log (i) 

= 

U6; 

end 


if (i>runtime) 
flag  =  1; 

end 

i=i+l 


end 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


RESET  VARIABLES  AND  PROPERLY  TERMINATE  PROGRAM%%%% 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


flag  =  0; 

avg  time  =  mean (frame  times (3: end)  -  frame  times (2 : end-1 ) ) 
%close  all 

%plot (x_m_p*pose_hist ( : , 1 ) ,  y_m_p*po  se_hist (:, 2 ) 
%grid  on; 


%  This  is  frame  time 

%plot (frame  times(3:end)  -  frame  times (2 : end-1 ) ) 


delete (vidobj ) ; 

if  (communication  ==1) 

fclose ( s3 ) ; 

delete ( s3 )  ; 

end 


16.3  Controller  III  Code 
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16.3.1  Controller  III  Simulation  Code 


Simulate: 

global  ul4  age 
global  u25_age 
global  u36_age 
ul4_age  =  0.0; 
u25_age  =  0.0; 
u36_age  =  0.0; 

LogFreq  =  1; 

sim ( ' SwarmModel ' , [0:0.25:200]); 

ParseData 

plot_angle 

SwarmDynKin: 

function  [ sys , xO , str , ts ]  =  SwarmDynKin (t, x, u, flag) 
switch  flag, 

O, _ 

Q - 

%  Initialization 

g, _ 

o - 

case  0, 

[sys,x0,str,ts] =mdl Initialize Sizes; 

O, _ 

Q - 

%  Derivatives 

g, _ 

0 - 

case  1, 

sys=mdlDerivatives (t, x, u) ; 

g, _ 

Q - 

%  Update 

g, _ 

Q - 

case  2, 

sys=mdlUpdate (t, x, u) ; 

g, _ 

Q - 

%  Outputs 

g, _ 

Q - 

case  3, 

sys=mdlOutputs (t, x, u) ; 

O, _ 

o - 

%  GetTimeOfNextVarHit 

g, _ 

o - 

case  4, 

sys=mdlGetTimeOfNextVarHit (t, x, u) ; 
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g, _ 

o - 

%  Terminate 

g, _ 

0 - 

case  9, 

sys=mdlTerminate (t, x, u) ; 

O, _ 

o - 

%  Unexpected  flags 

g, _ 

Q - 

otherwise 

error ([' Unhandled  flag  =  ' , num2str (flag) ] ) ; 

end 

g,  g, _ 

o  o - 

%  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-function 

g _ _ 

o - 

function  [sys,xO,str,ts] =mdl Initialize Sizes 

%  Call  simsizes  for  a  sizes  structure,  fill  it  in  and  convert  it  to  a  sizes 
%  array. 

%  Note  that  in  this  example,  the  values  are  hard  coded.  This  is  not  a 
%  recommended  practice  as  the  characteristics  of  the  block  are  typically 
%  defined  by  the  S-function  parameters. 

sizes  =  simsizes; 

sizes . NumContStates  =  15; 
sizes . NumDiscStates  =  0; 
sizes .NumOutputs  =  33; 

sizes . Numlnputs  =  0; 

sizes . DirFeedthrough  =  1; 

sizes . NumSampleTimes  =  1;  %  at  least  one  sample  time  is  needed 

sys  =  simsizes (sizes) ; 

theta_hat_IC  =  [1.0  0  0  0  1  1  0  0.19  0]  ; 

xO  =  [5.59  0.5  83.6*pi/180  0.0  0.0  0.0  theta_hat_IC] ' ; 

%  str  is  always  an  empty  matrix 
str  =  [ ] ; 

%  Initialize  the  array  of  sample  times 
ts  =[00]; 

%  end  mdllnitializeSizes 

g,  g, _ 

o  o - 

%  mdlDerivatives 

%  Return  the  derivatives  for  the  continuous  states. 

g _ _ 

o - 

function  sys=mdlDerivatives (t, x, u) 
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%  Exchange  of  variables 


Px 

=  x(l,l) 

r 

Py 

=  x(2,l) 

r 

psi 

=  x(3,l) 

r 

Vx 

=  x  ( 4 , 1 

)  ; 

Vy 

=  x  (5, 1 

)  ; 

Vpsi 

=  x  (6, 1 

) ; 

B1  = 

x (7, 1)  ; 

B2  = 

x  ( 8 ,  1 )  ; 

B3  = 

x(9,  1)  ; 

B4  = 

x (10, 1) 

r 

B5  = 

x(ll,l) 

r 

B6  = 

x (12, 1) 

r 

B7  = 

x (13, 1) 

r 

B8  = 

x (14, 1) 

r 

B9  = 

x (15, 1) 

r 

V  = 

[Vx  Vy 

Vpsi]  ' 

P  = 

[Px  Py 

psi]  '  ; 

theta_hat  =  [B1  B2  B3  B4  B5  B6  B7  B8  B9] 

theta  hat  min  =  0.1; 

if (theta  hat(l)<=  theta  hat  min) 
theta  hat(l)  =  theta  hat  min; 

end 

if (theta  hat(5)<=  theta  hat  min) 
theta  hat  (5)  =  theta  hat  min; 

end 

if (theta  hat(6)<=  theta  hat  min) 
theta  hat (6)  =  theta^hat  min; 

end 

if (theta  hat(8)<=  theta  hat  min) 
theta  hat (8)  =  theta^hat  min; 

end 

[theta_hat_dot, dP, dV, U, e, r, B_hat]  =  DynamicsControl (theta_hat, P, V) ; 

sys  =  [  dP;  dV;  theta_hat_dot] ; 

%  end  mdlDerivatives 


%  mdlUpdate 

%  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
%  requirements. 

O, 

o 


function  sys=mdlUpdate (t, x, u) 
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sys  =  [  ]  ; 

%  end  mdlUpdate 


%  mdlOutputs 

%  Return  the  block  outputs. 

o, 

o 


function  sys=mdlOutputs (t, x, u) 
%  Exchange  of  variables 


Px 

=  x(l,l) 

r 

Py 

=  x(2,l) 

r 

psi 

=  x(3,l) 

r 

Vx 

=  x  ( 4 , 1 

)  ; 

Vy 

=  x  (5, 1 

)  ; 

Vpsi 

=  x  (  6, 1 

) ; 

B1  = 

x (7, 1)  ; 

B2  = 

x  (  8 ,  1 )  ; 

B3  = 

x  ( 9 , 1 )  ; 

B4  = 

x (10, 1) 

r 

B5  = 

x(ll, 1) 

r 

B6  = 

x (12, 1) 

r 

B7  = 

x (13, 1) 

r 

B8  = 

x (14, 1) 

r 

B9  = 

x (15, 1) 

r 

V  = 

[Vx  Vy 

Vpsi]  ' 

P  = 

[Px  Py 

psi]  '  ; 

theta_hat  =  [B1  B2  B3  B4  B5  B6  B7  B8  B9] 

theta  hat  min  =  0.1; 

if (theta  hat(l)<=  theta  hat  min) 
theta  hat(l)  =  theta^hat  min; 

end 

if (theta  hat(5)<=  theta  hat  min) 
theta  hat  (5)  =  theta^hat  min; 

end 

if (theta  hat(6)<=  theta  hat  min) 
theta  hat (6)  =  theta  hat  min; 

end 

if (theta  hat(8)<=  theta  hat  min) 
theta  hat (8)  =  theta^hat  min; 

end 


[theta  hat  dot, dP, dV, U, e, r, B  hat]  =  DynamicsControl (theta  hat,P,V); 
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%  Output  Vector 

Q, 

O 


sys  =  [ P ' , U ' , e ' , theta_hat ' , r ' , B_hat ( 1 , : ) , B_hat ( 2 , : ) , B_hat ( 3 , : ) ] ; 
%  end  mdlOutputs 


%  mdlGetTimeOfNextVarHit 

%  Return  the  time  of  the  next  hit  for  this  block.  Note  that  the  result  is 
%  absolute  time.  Note  that  this  function  is  only  used  when  you  specify  a 
%  variable  discrete-time  sample  time  [-2  0]  in  the  sample  time  array  in 
%  mdllnitializeSizes . mdl 

O, 

o 


function  sys=mdlGetTimeOfNextVarHit (t, x, u) 

sampleTime  =1;  %  Example,  set  the  next  hit  to  be  one  second  later, 

sys  =  t  +  sampleTime; 

%  end  mdlGetTimeOfNextVarHit 


%  mdlTerminate 

%  Perform  any  end  of  simulation  tasks. 

O, 

o 


function  sys=mdlTerminate (t, x, u) 
sys  =  [ ] ; 

%  end  mdlTerminate 


DynamicsControl: 

function  [theta_hat_dot, dP, dV, U, e, r, B_hat]  =  DynamicsControl (theta_hat, P, V) 

o, _ 

O - 

%  System  Parameters  and  Matrix  Definitions 

O, _ 

O - 

%  Tugboat  locations 
global  ul4  age 
global  u25  age 
global  u36_age 

rl  =  0.6; 

alphal  =  (180.0) *pi/180.0; 
thetal  =  (0 . 0) *pi/180 . 0; 

r2  =0.27; 

alpha2  =  (270 . 0) *pi/180 . 0; 

theta2  =  (44 . 72) *pi/180 . 0; 
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r3  =  0.19; 

alpha3  =  (270 . 0) *pi/180; 

theta3  =  (90 . 0) *pi/180 . 0; 

r4  =  rl; 

alpha4  =  (0 . 0) *pi/180 . 0; 
theta4  =  (180 . 0) *pi/180 . 0; 

r5  =  r2  ; 

alpha5  =  (90 . 0) *pi/180 . 0; 
theta5  =  (360 . 0) *pi/180 . 0-theta2; 

r6  =  r3; 

alpha6  =  (90 . 0) *pi/180 . 0; 
theta6  =  (270 . 0) *pi/180 . 0; 

%B  member  signs  needed  for  adaptive  laws  to  work%%%%%%% 

bll  sgn  =  -1; 

bl2_sgn  =  0; 

bl3_sgn  =  0; 

b21_sgn  =  0; 

b22  sgn  =  -1; 

b23_sgn  =  -1; 

b31_sgn  =  0; 

b32_sgn  =  -1; 

b33_sgn  =  0; 

dl  =  0.05; 
d2  =  0.05; 
d3  =  0.15; 


D  =  [dl  0  0;0  d2  0;0  0  d3] ; 


%  Mass  matrix 
m  =  15.5129;  %  (kg) 

Iz  =  1.5849;  %  (kgmA2) 

M  =  [m  0  0; 

0  m  0 ; 

0  0  Iz] ; 


%%  Control 
Px  =  P  ( 1 ,  1 )  ; 

Py  =  P(2, 1)  ; 
psi  =  P (3, 1)  ; 

%  Rotation  Matrix 
R  =  [cos (psi)  -sin (psi)  0; 


sin(psi)  cos(psi)  0; 

0  0  1  ]  ; 

Pdot  =  R*V; 

%  Desired  Trajectories 

Pd  =  [2  2  (90.0) *pi/180 . 0]  '  ; 

PdDot  =  [0.0  0.0  0.0]  '  ; 

PdDDot  =  [0.0  0.0  0.0]'; 

%  Control  gains 
gammaO  =  0.0; 

%Kp  =  0.05; 

%  Kr  =0.5; 

%  alpha  =  1.0; 

%  Kr  =  [.600; 

%  0.6  0; 

%  00.8]; 

Kr  =  [2  0  0; 

0  5  0; 

0  0  4]; 

alpha  =  [ . 3  0  0 ; 

0  .3  0; 

0  0  .35] ; 


%theta  =  [-1  000-1-10  -r2*cos (theta2 )  0] ' 
B  =  [-1  0  0; 

0  -1  -1; 

0  -r2*cos (theta2)  0] ; 

%  theta  =  [(B(l,l))  (B(l,2))  (B(l,3)) 

%  (B  (2 , 1 )  )  (B  (2 , 2 )  )  (B (2, 3)  ) 

%  (B (3, 1)  )  (B (3, 2)  )  (B  (3,3)  )  ]  '  ; 


%%update  Y 

theta  hat  min  =  0.1; 

if (theta  hat(l)<=  theta  hat  min) 
theta  hat(l)  =  theta  hat  min; 

end 

if (theta  hat(5)<=  theta  hat  min) 
theta  hat  (5)  =  theta  hat  min; 

end 

if (theta  hat(6)<=  theta  hat  min) 
theta  hat  (6)  =  theta  hat  min; 


end 
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if (theta  hat(8)<=  theta  hat  min) 
theta  hat (8)  =  theta  hat  min; 

end 

B  hat  =  [bll_sgn* (theta  hat(l))  bl2  sgn* (theta  hat (2)) 
bl3_sgn* (theta_hat (3) )  ; 

b21_sgn* (theta_hat (4) )  b22_sgn* (theta_hat (5) ) 
b23_sgn* (theta_hat (6) ) ; 

b31_sgn* (theta_hat (7) )  b32_sgn* (theta_hat (8) ) 
b33_sgn* (theta_hat (9) ) ] ; 


Y  =  [bll_sgn* (cos (psi) *ul4_age) /m,  bl2_sgn* (cos (psi) *u25_age) /m, 
bl3_sgn* (cos (psi) *u36_age) /m, . . . 

b21  sgn* (sin (psi) *ul4  age) /m,  b22  sgn* (sin (psi) *u25  age) /m, 
b23_sgn* (sin (psi) *u36_age) /m,  0,  0,  0; 

bll  sgn*- (sin (psi) *ul4  age) /m,  bl2  sgn*- (sin (psi) *u25  age) /m,  bl3  sgn*- 
(sin (psi) *u36_age) /m, . . . 

b21_sgn* (cos (psi) *ul4_age) /m,  b22_sgn* (cos (psi) *u25_age) /m, 
b23_sgn* (cos (psi) *u36_age) /m,  0,  0,  0; 

0,  0,  0,  0,  0,  0,  b31_sgn*ul4_age/Iz,  b32_sgn*u25_age/Iz, 
b33_sgn*u36_age/Iz] ; 

%  Error  signals 
e  =  Pd-P; 
eDot  =  PdDot-Pdot; 
r  =  eDot+alpha*e; 


theta  hat  dot  =  -l*Y'*r; 


if  (theta  hat(l)  <=  theta  hat  min  &&  theta  hat  dot(l)<0.0) 
theta_hat_dot ( 1 )  =  0.0; 

end 

if  (theta_hat (5)  <=  theta_hat_min  &&  theta_hat_dot ( 5 ) <0 . 0 ) 
theta_hat_dot ( 5 )  =  0.0; 

end 

if  (theta_hat (6)  <=  theta_hat_min  &&  theta_hat_dot (6) <0 . 0) 
theta_hat_dot ( 6)  =  0.0; 

end 

if  (theta_hat (8)  <=  theta_hat_min  &&  theta_hat_dot ( 8 ) <0 . 0 ) 
theta  hat  dot (8)  =  0.0; 


end 


S  =  [0  10; 

-1  0  0; 

0  0  0  ]  ; 
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Us 

inv (R*inv (M) *B  hat) * ( PdDDot+alpha*eDot+P (3,1) *S*Pdot+Kr*r+R*inv (M) *D*R' *Pdot) 


ul4  =  Us (1, 1)  ; 
u2  5  =  Us  (2 , 1)  ; 
u36  =  Us (3, 1)  ; 

ul4_age  =  ul4; 
u25_age  =  u25; 
u36_age  =  u36; 

ul  =  0.5* (ul4  +sqrt (ul4A2+gammaOA2) ) ; 
u4  =  0 . 5* (~ul4+sqrt (ul4A2+gammaOA2) ) ; 

u2  =  0.5* (u25  +sqrt (u25 A2+gamma0 A2 ) ) ; 
u5  =  0 . 5*  (~u25  +  sqrt (u25A2+gammaOA2) ) ; 

u3  =  0.5* (u36  +sqrt (u36A2+gammaO A2 ) ) ; 
u6  =  0 . 5* (-u36+sqrt (u36A2+gammaO A2 ) ) ; 

if  (ul>2 . 4 ) 
ul  =  2.4; 

end 

if  (u4>2 . 4 ) 
u4  =  2.4; 

end 

if  (u2>l . 6) 

u2  =  1.6; 

end 

if  (u3>l . 6) 
u3  =  1.6; 

end 

if  (u5>l . 6) 
u5  =  1.6; 

end 

if  (u6>1.6) 
u  6  =  1.6; 

end 


U  =  [ul  u2  u3  u4  u5  u6]  '; 

%%  Kinematics 
dP  =  R*V; 

%%  Swarm/Barge  System  Dynamics 
%dV  =  inv (M) * ( -D*V+M*R ' *Y* theta) ; 
dV  =  inv (M) * (-D*V+B*Us) ; 
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return; 


16.3.2  Con  troller  III  Experimen  tal  Code 

AdaptiveBexp : 

clear  M 
clear  all 
close  all 
clc 

%desired  location  and  orientation  in  pixels 
x  des=input (' Input  the  desired  X  position'); 
y_des=input (' Input  the  desired  Y  position'); 
angle  des=input (' Input  the  desired  angle'); 
frame  number  =  input (' Input  the  run  time'); 

O _ 

O - 

%initialize  variables  and  vessel  parameters 

o _ 

O - 

x_log  =  [];  y_log  =  [];  si_log  =  [];  Ul_log  =  [];  U2_log  =  []; 
U3_log  =  [];  U4_log  =  [];  U5_log  =  [];  U6_log  =  [];  time_log  =  []; 
B_hat_log  =  []; 
x  conversion  =  7.927/640; 
y  conversion  =  4.4/480; 

theta  hat  min  =  0.05; 


%  Kr  =  [.7  0  0; 

%  0.60; 

%  00.3]; 

%  k^alpha  =  [.17  0  0; 

%  0  .17  0; 

%  00.3]; 

Kr  =  [ . 2  0  0 ; 

0  .5  0; 

0  0  .4]; 

k_alpha  =  [.3  0  0; 

0  .3  0; 

0  0  .35] ; 

%  al  =  180*pi/180; 

%  a2  =  270*pi/180; 

%  a3  =  270*pi/180; 

%B  member  signs  needed  for  adaptive  laws  to  work%%%%%%% 

bll  sgn  =  -1; 

bl2_sgn  =  0; 

bl3_sgn  =  0; 

b21_sgn  =  0; 

b22  sgn  =  -1; 

b23_sgn  =  -1; 

b31_sgn  =  0; 

b32_sgn  =  -1; 

b33_sgn  =  0; 
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j  =  1.5849;  %using  moment  of  inertia  formula  for  a  cuboid  from 

%http : / / www . diracdelta .co.uk/ science/ source/m/ o /moment s%2 Oof %20 inertia/ source 

.  html 

m  =  15.5129; 

%  rl  =  0.6; 

%  r2  =  0.27; 

%  r3  =  0.19; 
si  =  0  ; 

%  tl  =  0*pi/180; 

%  t2  =  44 . 72*pi/180; 

%  t3  =  90*pi/180; 
xd  =  x_de  s ; 
yd  =  y_de  s ; 

sid  =  angle_des*pi/180; 
gamO  =  0 ; 
error_age  =  0; 
timer  =  0; 


time 

age  = 

:  0 

r 

si  = 

t 

0 

1 

0; 

- 

■1 

0 

0; 

0 

0 

0]  ; 

Mass 

= 

[m 

0 

0; 

0 

m 

0; 

0 

0 

j  ]  ; 

dl  = 

0. 

LO 

o 

d2  = 

0. 

LO 

o 

d3  = 

0. 

15; 

Drag 

_ 

[dl 

0 

0; 

0 

d2 

0; 

0 

0 

d3  ] 

Ua_age  =  0.0; 
Ub_age  =  0.0; 
Uc_age  =  0.0; 

%  gamma  =[100 
%  0  10 

%  001]; 


flag  =  0; 


o _ 

0 - 

%set  up  com 

o _ 

Q - 


link 


format  compact 
plotimage  =  1; 
communication  =  1; 
if  (communication  ==1) 
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s3  =  serial (' C0M1 ',' baudrate ', 1 9200 ) ; 
f open ( s3 ) 

end 

xd  pix  =  xd*x  conversionA-l ; 
yd  pix  =  yd*y  conversionA-l ; 

O _ 

O - 

%initialize  camera 

o _ 

O - 

%  I  think  camera  needs  to  be  plugged  in  and  creative  cam  software  is  off 
before  you  start  matlab 
%  set  up  video  object 
vidobj  =  videoinput (' winvideo ') ; 

%  now  you  must  trigger  it  to  log  data 
triggerconf ig (vidobj , 'manual ' ) ; 

%  only  log  a  single  frame 

set (vidobj ,  ' f ramespertrigger '  ,  1); 

%  lets  you  trigger  it  as  many  times  as  you  want 
set (vidobj ,  ' triggerrepeat ' ,  inf) 

pose_hist  =  []; 

%start  but  don't  trigger  (log) 
start (vidobj ) 

%  wait  for  warm  up 
pause  ( 1 ) 

frame_times  =  []; 

i  =  1; 

tic; 

while (flag==0) 


o _ 

O - 

%GETS  VIDEO  FEED  AND  DETERMINES  THE  VEHICLES  POSITION  AND  ATTITUDE 

O _ 

o - 

%log  a  frame 
trigger (vidobj ) ; 

%  load  it  into  memory  with  time  stamp 
[frame  time]  =  getdata (vidobj ) ; 

%  just  keeping  track  of  how  many  frames  per  sec  we  are  getting 
frame  times  =  [frame  times;  time]; 

if (1==1) 

%  define  robots  are  a  certain  region  of  color  space 

green  =  frame(:,:,l)  >230  &  f rame ( : , : , 2 ) <1 60  &  frame ( : , : , 3) <150; 

%actually  this  is  red 

red  =  frame (:,:,1)  <  130  &  frame ( : , : , 2 ) >220  &  f rame ( : , : , 3 ) >50 ;  %actually 
this  is  green 

%  looking  for  8  connected  comopnents 
R  =bwlabel ( red, 8 ) ; 

Y  =bwlabel (green,  8 )  ; 

%  using  the  labeled  matrix  will  use  Dunbar's  optimized  property  finder 
%  this  is  the  built  in  version 
s  =  regionprops (R,  'centroid',  'area'); 
t  =  regionprops (Y,  'centroid',  'area'); 

%for  red  led's 
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centroids_L  =  cat(l,  s. Centroid); 
area_L  =  cat(l,  s.Area); 
robots_L  =  find (area_L>50) ; 

%for  white  led's 

centroids_R  =  cat(l,  t. Centroid); 
area_R  =  cat(l,  t.Area); 
robots_R  =  find (area_R>50) ; 

%  robots  have  to  be  bigger  than  some  #  of  pixels  to  eliminate 
%  spurrious  results 

%can  display  to  screen  at  cost  of  computation  time ...  actually  this 
%barely  impacts  it 
end 

if  (plotimage  ==  1) 

if  (plotimage  ==  1) 
image sc (frame) 
figure ( 1 ) 
hold  on 


end 

if  -isempty ( robots_R) 
if  -isempty (robots_L) 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


PLOTS  THE  POSITION  AND  ORIENTATION  ON  THE  IMAGE%%% 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


if  (plotimage  ==  1) 


plot (centroids_R (robots_R, 1 ) ,  centroids_R (robots_R, 2 ) ,  ' k*  '  )  ; 


plot (xd_pix,  4  8  0-yd_pix,  ' r+ ' ) ; 

plot ( [xd_pix+50*cos ( sid)  xd_pix-50*cos ( sid) ] ,  [480— 

(yd_pix+50*sin (sid) )  480- (yd_pix-50*sin (sid) ) ] ) ; 


plot (centroids_L (robots_L, 1 ) ,  centroids_L (robots^L, 2 ) ,  ' c* ' ) ; 

plot ( [centroids_R ( robots_R ( 1 ) , 1 )  centroids_L (robots_L ( 1 ) , 1 ) ] . . . 
,  [centroids  R(robots  R(l),2)  centroids  L(robots  L(l),2)], 


plot (1/2* (centroids_R ( robots_R ( 1 ) , 1 ) - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 ) , 1 ) ... 

,  1/2* (centroids_R ( robots_R ( 1 )  ,2) - 
centroids_L ( robots_L ( 1 ) ,2) ) +centroids_L ( robots_L ( 1 ) , 2 ) ,  ' r* ' ) ; 

end 

if  (ikframe  number) 

M(i)  =  getframe; 

end 

%from  green  to  red  LED  measured  from  normal  x-axis  on  cartesian 
%coordinates 

si  =  atan2 (centroids_R (robots_R ( 1 ) , 2 ) - (centroids_L (robots_L ( 1 ) , 2 ) ) . . . 
, (centroids_L ( robots_L ( 1 ) , 1 ) ) -centroids_R (robots__R (1) , 1) ) *180/pi; 


%makes  sure  angle  is  from  0  to  360  degrees 
if  (si  <  0) 

si  =  si  +  360; 
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end 

%converts  to  radians 
si  =  si*pi/180; 

error  si  =  sid-si; 
if  (error  si  >  pi) 

error  si  =  error  si- (360*pi/180) ; 

end 

if  (error  si  <  -pi) 

error_si  =  error_si+ (360*pi/180) ; 

end 


pos  =  [ 1 /2* (centroids_R (robots_R ( 1 )  ,  1 ) - 
centroids_L ( robots_L ( 1 ) , 1 ) ) +centroids_L ( robots_L ( 1 )  ,  1 ) 

,  1/2*  (centroids_R (robots_R ( 1 )  ,  2 ) - 
centroids_L ( robots_L ( 1 ) , 2 ) ) +centroids_L ( robots_L ( 1 ) , 2 ) ] ; 
x_pose  =  pos(l); 
y_pose  =  480-pos(2); 

%Logs  x,y,si,and  time 
x_log (i) =x_pose; 
y_log (i) =y_pose; 
si_log(i)  =  si; 
timer  =  toe; 
time  log(i)  =  timer; 

end 

end 


9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


DEFINE  MEMBER  MATRICIES 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

ooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Rot  =  [cos (si)  -sin(si)  0  ; 

sin ( si )  cos  ( si )  0  ; 

0  0  1]; 


%converts  pixels  to  m 
x  =  x  pose*x  conversion; 
y  =  y  pose*y  conversion; 


%  vectors 

error  =  [xd-x  yd-y  error  si] '; 


Pd  =  [xd  yd  sid] ' ; 
PI  =  [x  y  si]  '  ; 
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%CALCULATE  ERROR  DERIVATIVE 


%  derivatives  using  rise  over  run  technique  and  integrals 
%  using  the  trapazoidal  rule 
%  B  =  [-1  0  0; 

%  0-1  -1; 

%  0  -r2*cos (theta2 )  0] ; 

if (i — 1) 

theta_hat  =  [1.5  0  0  0  .5  .7  0  .3  0  ]  ’  ; 
end 

B  hat  =  [bll  sgn*theta  hat(l)  bl2  sgn*theta  hat (2)  bl3  sgn*theta  hat (3); 

b21  sgn*theta  hat (4)  b22  sgn*theta  hat (5)  b23  sgn*theta  hat (6); 
b31_sgn*theta_hat (7)  b32_sgn*theta_hat (8)  b33_sgn*theta_hat (9) ] ; 

Y_adp  =  [bll_sgn* (cos (si) *Ua_age) /m,  bl2_sgn* (cos (si) *Ub_age) /m, 
bl3_sgn* (cos (si) *Uc_age) /m, . . . 

b21_sgn* (sin (si) *Ua_age) /m,  b22_sgn* (sin (si) *Ub_age) /m, 
b23_sgn* (sin (si) *Uc_age) /m,  0,  0,  0; 

bll_sgn*- (sin (si) *Ua_age) /m,  bl2_sgn*- (sin (si) *Ub_age) /m,  bl3_sgn*~ 

(sin (si) *Uc_age) /m,  . .  . 

b21_sgn* (cos (si) *Ua_age) /m,  b22_sgn* (cos (si) *Ub_age) /m, 
b23_sgn* (cos (si) *Uc_age) /m,  0,  0,  0; 

0,  0,  0,  0,  0,  0,  b31_sgn*Ua_age/ j ,  b32_sgn*Ub_age/ j ,  b33_sgn*Uc_age/ j ] ; 
if  (i==l) 

error_dot  =  [0  0  0] 

Pd_dot  =  [0  0  0]  '  ; 

Pl_dot  =  [0  0  0]  '  ; 

Pd_dot_dot  =  [0  0  0] 

else 

error  dot  =  (error-error  age) / (timer-time  age) ; 

Pd_dot  =  (Pd-Pd_age) / (timer-time_age) ; 

Pl_dot  =  (Pl-Pl_age) / (timer-time_age) ; 

Pd_dot_dot  =  (Pd_dot-Pd_dot_age) / (timer-time_age) ; 

end 


%error  signal  r 

r  error  =  error  dot  +  k  alpha  *  error; 

%%%%%%%%need  to  set  up  integrator 
theta  hat  dot  =  1.0*-Y  adp'*r  error; 

%  theta_hat_dot ( 6)  =  10*theta_hat_dot ( 6) ; 

if  (theta  hat(l)  <=  theta  hat  min  &&  theta  hat  dot(l)<0.0) 
theta_hat_dot ( 1 )  =  0.0; 

end 


if  (theta  hat (5)  <=  theta  hat  min  &&  theta  hat  dot(5)<0.0) 


0.0; 
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theta_hat_dot ( 5 )  = 

end 


if  (theta_hat (6)  <=  theta_hat_min  &&  theta_hat_dot (6) <0 . 0) 
theta_hat_dot ( 6)  =  0.0; 

end 


if  (theta_hat (8)  <=  theta_hat_min  &&  theta_hat_dot ( 8 ) <0 . 0 ) 
theta_hat_dot ( 8 )  =  0.0; 

end 


if ( i~=l ) 

theta  hat  =  theta  hat+ ( (timer-time  age) * (theta  hat  dot  + 

theta_hat_dot_age) /2) ; 

end 

if (theta  hat(l)<=  theta  hat  min) 
theta  hat(l)  =  theta  hat  min; 

end 

if (theta  hat(5)<=  theta  hat  min) 
theta  hat (5)  =  theta^hat  min; 

end 

if (theta  hat(6)<=  theta  hat  min) 
theta  hat (6)  =  theta  hat  min; 

end 

if (theta  hat(8)<=  theta  hat  min) 
theta  hat  (8)  =  theta  hat  min; 

end 


%//================================================= 

%//implement  full  controller 

O, 

o  •  •  • 

%//================================================ 

U  =  inv (Rot*inv (Mass) *B  hat)* ((Pd  dot  dot+k  alpha*error  dot)... 

+(P1  dot(3)*sl*Pl  dot)+(Kr*r  error) + (Rot*inv (Mass) *Drag*Rot ' *P1  dot)); 
%Allocate  thrusts 
Ua  =  U  ( 1 )  ; 

Ub  =  U  ( 2  )  ; 

Uc  =  U  ( 3 )  ; 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  7\  P  T7  T1  HT7  T  DfSTTHM  T 1?  P IVT  Q  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9- 
ooooooooooooooooooooo  oiAoJli  -L  nj_i  1U1N  ±J_ir\ivlQ  oooooooooooooooooooooooooooo 

9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

Pd_age  =  Pd; 

Pipage  =  PI; 

Pd_dot_age  =  Pd_dot; 
error  age  =  error; 
time  age  =  timer; 

theta  hat  dot  age  =  theta  hat  dot; 


o\°  o\°  o\° 
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Ua_age  =  Ua; 
Ub_age  =  Ub ; 
Uc_age  =  Uc; 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'QTh1T\TPi  THDT1QTQ  Tfl  TUTT  RH  A  T  2-  9-  9-  9-  9-  9-  S-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9-  9- 
ooooooooooooooooooooo  o  O  Hi  IN  U  ini\UijiiJ  -L  ±  n.J_j  owr^.1  oooooooooooooooooooooooooo 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

if  (communication  ==1) 


my_stringl  =  sprintf ( ' a%5 . 3f \r 1 ,  Ua)  ; 
fprintf(s3,  my  stringl); 

my_string2  =  sprintf (' b%5 . 3f\r ' ,  Ub) ; 
fprintf(s3,  my  string2); 

my_string3  =  sprintf (' c%5 . 3f\r ' ,  Uc) ; 
fprintf(s3,  my_string3); 


end 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

ooooooooooooooooooooo 


%CALCULATE  AND  LOG  INDIVIDUAL  THRUSTS 


S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


Ul  =  0 . 5* (Ua+sqrt (Ua*Ua+gamO*gamO) ) ; 

U4  =  abs ( 0 . 5* ( -Ua+sqrt (Ua*Ua+gamO*gamO ))) ; 

U2  =  0 . 5* (Ub+sqrt (Ub*Ub+gamO*gamO ) ) ; 

U5  =  abs (0 . 5* (-Ub+sqrt (Ub*Ub+gamO*gamO ) )) ; 

U3  =  0 . 5* (Uc+sqrt (Uc*Uc+gamO*gamO) ) ; 

U6  =  abs ( 0 . 5* ( -Uc+sqrt (Uc*Uc+gamO*gamO ))) ; 

if  (Ul>2 . 4 ) 

Ul  =  2.4; 

end 

if  (U4>2 . 4 ) 

U4  =  2.4; 

end 

if  (U2>1 . 6) 

U2  =  1.6; 

end 

if  (U3>1 . 6) 

U3  =  1.6; 

end 

if  (U5>1 . 6) 

U5  =  1.6; 

end 

if  (U6>1 . 6) 

U  6  =  1.6; 

end 
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Ul_log(i)  = 
U4_log(i)  = 
U2_log(i)  = 
U5_log(i)  = 
U3_log(i)  = 
U6_log(i)  = 

B_hat_log ( 1 , 
B_hat_log (2 , 
B_hat_log ( 3 , 
B_hat_log ( 4 , 
B_hat_log ( 5 , 
B_hat_log (6, 
B_hat_log ( 7 , 
B_hat_log ( 8 , 
B_hat_log (9, 

end 


Ul; 

U4  ; 

U2  ; 

U5  ; 

U3  ; 

U6; 

i )  =  B_hat  (1,1)  ; 
i )  =  B_hat  (1,2)  ; 
i )  =  B_hat (1,3)  ; 
i)  =  B_hat  (2,1); 
i)  =  B_hat  (2,2)  ; 
i)  =  B_hat (2,3)  ; 
i )  =  B_hat  (3,1); 
i )  =  B_hat  (3,2)  ; 
i )  =  B_hat (3,3)  ; 


if(i>frame  number) 
flag  =  1; 

end 
i=i  +  l  ; 


end 


9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'2'9'9'9'2'9'9'9'9'9'9'9'2'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9'9' 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooo 


RESET  VARIABLES  AND  PROPERLY  TERMINATE 


PROGRAM%%% 


Q„ 

O 


S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


flag  =  0; 

avg  time  =  mean (frame  times(3:end)  -  frame  times (2 : end-1 ) ) 
%close  all 


%plot (x_m_p*pose_hist ( ; , 1 ) ,  y_jn_p*po  se_hist (:, 2 ),'*-' ) 
%grid  on; 


%  This  is  frame  time 

%plot(frame  times(3:end)  -  frame  times (2 : end-1 ) ) 


delete (vidobj ) ; 

if  (communication  ==1) 

fclose ( s3 ) ; 

delete ( s3 )  ; 

end 


Performance  Index 


Performance  index  rev: 
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%Calculates  the  error  index,  thrust  index,  and  settling  time 
run  it  =  151; 

x  log  pt  =  x  log*7 . 927/640; 
y_log_pt  =  y_log*4 . 4/480; 

Pm  =  0.0; 

Ti  =  0.0; 
flag  =  0; 
stopflag  =  0; 

for  i  =  1 : run  it 

%construct  the  error  vector 

error (:,i)  =  Pd- [x_log_pt ( i )  y_log_pt(i)  si_log(i)]'; 
error(3,i)  =  error (3, i) *0 . 46; 

%calculate  the  error  index 
Pm  =  Pm  +  norm (error (:, i )) ; 

%calculate  the  thrust  index 

Ti  =  Ti  +  Ul_log ( i ) +U2_log ( i ) +U3_log ( i ) +U4_log ( i ) +U5_log ( i ) +U6_log ( i ) ; 
%calculate  the  settling  time 

if  (norm (error (:, i ) )  <=  0.44  &&  stopflag==0) 
flag  =  flag+1; 
if  (flag>5) 

t_settle  =  time_log(i) 
stopflag  =  1; 

end 

end 


end 

%normalize  the  error  and  thrust  indicies 
Performance  =  Pm/run  it 
Thrust  avg  =  Ti/run  it 

16.5  Vessel's  C-code 
Gaincontrolremote.c : 

//  ================================================================== 

//  Name  :  Erik  T  Smith 

//  Description  :  Tugboat  Position  control,  Closed-Loop,  Position  and 
//  Heading  Control 

//  Date  :  08-FEB-2007 

//  Slave  program  to  Matlab,  outputs  counts  when  given  thrusts 
//  ================================================================== 


//  ==================== 

//  Rabbit  Parameters 
//  ==================== 

#def ine  BOUTBUFSIZE  127 
#def ine  BINBUFSIZE  127 
#def ine  EOUTBUFSIZE  127 
#def ine  EINBUFSIZE  127 
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#def ine  COUTBUFSIZE  127 
#def ine  CINBUFSIZE  127 


/ /  ================================ 

//  Sensor  Coefficients 
//  ================================ 

#define  pi  (3.1415926) 

//  pi 


//  =================================================== 

//  Prototypes 

//  =================================================== 

xmem  nodebug  void  MsDelay (int)  ; 

xmem  nodebug  void  move_servos (int, int, int, int,  int,  int) ; 

//  =================================================== 

//  Declare  Global  Variables 

/ /  =================================================== 


// 


//  Main() 

// 


void  main (void) 

{ 

/ /  =================================================== 

//  Local  variable  declarations 
/ /  =================================================== 

int  si,  s2,  s3,  s4,  s5,  s6; 

char  sentence  [20] ; 

char  inchar [ 12 ]; 

char  outchar[6]; 

float  enable; 

char  recCmd; 

char  c; 

int  z,  i; 

long  count; 

float  Ua,  Ub,  Uc,  Ul,  U2,  U3,  U4,  U5,  U6,  gamO,  exit,  e_time; 


//  =====================: 

//  Initialize  Rabbit  SBC 
//  =====================: 


// 


set  baud  rate  for  each  serial  port 
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serEopen (9600) ; 
serBopen (19200)  ; 
serCopen (9600) ; 


serEwrFlush ( ) ; 
serErdFlush ( ) ; 
serBwrFlush ( ) ; 
serBrdFlush ( ) ; 
serCwrFlush ( ) ; 
serCrdFlush ( ) ; 


// - turn  off  all  motors  that  may  be  running 


move  servos (128,  128,  128,  128,  128,  128); 


// - initialize  variables 


i=0 ; 

c  =  255; 

for (i=0; i<=ll; i++) 
inchar [i] = ' \0 ' ; 


for (i=0; i<=5; i++) 
outchar [ i ] = ' \0 ' ; 


i  =  0; 
z  =  0; 

exit  =  0.0; 

e  time  =  105000.0;  //repetitions  the  program  waits  before  shutting  the 

boat 

//down  when  no  update  is  rec  dfreeived 


gamO  =  0 ; 
Ua  =  0.0; 
Ub  =  0.0; 
Uc  =  0.0; 


//  =========: 

/ /  Main  loop 

//  =========: 
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while  ( 1 ) 

{ 

/ /  =================================================== 

//  Inputs  -  measurements 

/ /  =================================================== 

recCmd  =  'N';  //RECCMD  FLAG  IS  RESET  TO  NO 

while ( (recCmd  ==  ' N ' )  &&  (exit  <  e  time)) 

{ 

// - RECEIVE  COMMAND  LOOP - 


//waits  until  it  receives  a  character  other  than  ascii  255 

while (exit<e  time)  //  loop  until  we  get  a  character 

{ 

//reads  in  a  character  from  serial  port  B 

c  =  serBgetc ( ) ; 

//if  character  is  different  than  ascii  255  then  exit  loop 
if  (c  ! =  255) 

{ 

break; 
exit  =  0.0; 

} 

exit++; 

} 

// - CHARACTER  CHECK - 


//if  a  character  is  \r  then  the  string  is  received,  else  put  the 
character  in  inchar 

if (c  ==  13) 

{ 

recCmd  =  ' Y ' ; 
inchar [i]  =  ' \0  '  ; 
i=0  ; 

} 

else  if (c  ! = 1 0 )  //Matlab  appends  a  line  feed  (dec  10)  to 

each  output  so  must  not  input 

{ 

inchar [i]  =  (c  &  0x7F) ;  //forces  7  bit  ascii 

//  printf ( "%c" , c) ; 

i++; 

} 

//initialize  c  to  ascii  255 
c  =  255; 

} 

//========================================================================== 


//For  the  following  if  statements,  the  2nd  through  4th  elements  must  be 
digits  for  the  statement  to  get  converted  and  assigned 
//===================================================================== 
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if (inchar [0]  ==  97) 

//if  1st  character  in  string  is  an  'a'  then  inchar  is  saved  as  Ua 

{ 

for  (z=0; z<=4; z++) 

outchar[z]  =  inchar  [z  +  1]; 

Ua  =  atof (outchar) ; 
printf("Ua  =  %f\n",Ua); 
count++; 

} 

if (inchar [0]  ==  98) 

//if  1st  character  in  string  is  an  ' b '  then  inchar  is  saved  as  Ub 

{ 

for  (z=0; z<=4; z++) 

outchar [z]  =  inchar [z+1]; 

Ub  =  atof (outchar) ; 
printf("Ub  =  %f\n",Ub); 
count++; 

} 

if (inchar [0]  ==  99) 

//if  1st  character  in  string  is  an  ' c'  then  inchar  is  saved  as  Uc 

{ 

for  (z=0; z<=4; z++) 

outchar  [z]  =  inchar  [z  +  1]; 

Uc  =  atof (outchar) ; 
printf("Uc  =  %f\n",Uc); 
count++; 

} 


//  =============== 

/ /  Clear  variables 

//  =============== 


for (i=0; i<=5; i++) 
outchar [ i ] = ' \0 ' ; 
for (i=0; i<=ll; i++) 
inchar [i] = ' \0 ' ; 
i=0 ; 

//  ================ 

/ /  Calculate  counts 

II  ================ 


if (fmod ( (float) count, 3 . 0)  ==  0.0  &&  count !=0  &&  exit  <  e_time) 

{ 


count  =  0; 


//========================= 

//each  thrust  output  in  (N) 
//========================= 


U1  =  0 . 5* (Ua+sqrt (Ua*Ua+gamO*gamO) ) ; 

U4  =  fabs (0 . 5* (-Ua+sqrt (Ua*Ua+gamO*gamO) )) ; 

U2  =  0 . 5* (Ub+sqrt (Ub*Ub+gamO*gamO) ) ; 

U5  =  fabs (0 . 5* (-Ub+sqrt (Ub*Ub+gamO*gamO ) )) ; 

U3  =  0 . 5* (Uc+sqrt (Uc*Uc+gamO*gamO) ) ; 

U6  =  fabs (0 . 5* (-Uc+sqrt (Uc*Uc+gamO*gamO) )) ; 

if  (Ul>2 . 4 ) 

U1  =  2.4; 
if  (U4>2 . 4 ) 

U4  =  2.4; 
if  (U2>1 . 6) 

U2  =  1.6; 
if  (U3>1 . 6) 

U3  =  1.6; 
if  (U5>1 . 6) 

U5  =  1.6; 
if  (U6>1 . 6) 

U  6  =  1.6; 


//  printf ( "Ul=%f  U2=%f  U3=%f  U4=%f  U5=%f 
U6=%f\r\n",Ul,U2,U3,U4,U5,U6) ; 

/ / ====================================== 

//Calculate  counts  for  each  thruster 
//======================================: 


//counts  for  all  lOOOgph  thrusters  1,4 

//3rd  order  curve  fit  using  a  hat  =  109.2619,  b  hat  =  -72.8738 
c  hat  =  64.8534,  d  hat  =  -22.3762 

si—  (int) (109.2619  +  -72.8738*U1  +  64 . 8534*U1*U1  +  - 
22.3762*U1*U1*U1) ; 

s4=  (int) (109.2619  +  -72.8738*U4  +  64 . 8534*U4*U4  +  - 
22 . 3762*U4*U4*U4) ; 


//counts  for  all  800gph  thrusters  2, 3, 5, 6 

//3rd  order  curve  fit  using  a  hat  =  108.1623,  b  hat  =  -57.3317 
c_hat  =  46.4793,  d_hat  =  -31.0171 

s2=  (int)  (108.1623  +  -57.3317*U2  +  46 . 4793*U2*U2  +  - 
31.0171*U2*U2*U2) ; 

s3=  (int)  (108.1623  +  -57.3317*U3  +  46 . 4793*U3*U3  +  - 
31.0171*U3*U3*U3) ; 

s5=  (int)  (108.1623  +  -57.3317*U5  +  46 . 4793*U5*U5  +  - 
31.0171*U5*U5*U5) ; 

s6=  (int)  (108.1623  +  -57.3317*U6  +  46 . 4793*U6*U6  +  - 
31.0171*U6*U6*U6) ; 


//=============================== 

//force  all  counts  greater  than  1 
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if  (sl<l) 

si  =  1; 
if  ( s2<l ) 

s2  =  1; 
if  ( s3<l ) 
s3  =  1; 
if  ( s4<l ) 
s4  =  1; 
if  ( s5<l ) 
s  5  =  1  ; 
if  (s6<l) 

s6  =  1; 

move_servos(sl,  s2,  s3,  s4,  s5,  s6)  ; 

} 

if  (exit  >=  e  time) 

{ 

move_servos (128,  128,  128,  128,  128,  128); 
exit  =  0.0; 

} 


} 


} 


// 


//  Library  Functions 

// 


//////////////////////////////////////////////////////////////////// 

/***  Beginheader  MsDelay  */ 
void  MsDelay (int  MS); 

/***  endheader  */ 

/*  START  FUNCTION  DESCRIPTION  ******************************************** 
MSDelay  <ES308_SBC . LIB> 


SYNTAX:  MsDelay (int  MS); 


DESCRIPTION:  This  function  causes  processing  to  be  delayed  for  the 

specified 


costatement . 


number  of  milliseconds.  It  should  not  be  used  within  a 


(Use  waitfor (DelayMS (ms) )  in  costatements. 


136 


See  also  SecDelay (sec) . 

RETURN  VALUE:  None 

END  DESCRIPTION  **********************************************************/ 

xmem  void  MsDelay(int  MS)  //  Millisecond  delay 

{ 

//  12/13/02  Modified  with  code  below  V2 . 7  -  wml 

auto  unsigned  long  done^time; 
done_time  =  MS_TIMER  +  MS; 

while ((long) (MS  TIMER  -  done  time)  <0)  {/*  do  nothing  */} 

//  Versions  before  V2 . 7 
//long  SavTimer,  TimerDiff; 

//TimerDiff  =  0; 

//SavTimer  =  MS_TIMER; 

//while (TimerDiff  <  MS)  {TimerDiff  =  MSJIIMER  -  SavTimer;} 

} 

tmemmap  xmem 


xmem  nodebug  void  move_servos (int  si,  int  s2,  int  s3,int  s4,  int  s5,  int  s6) 
//SV203  Board 
{ 

char  temp [10] ; 

serEputs ("BD1") ;  //initializes  board  #  1 
serEputs ("SV1") ;  //sends  count  number  to  servo  1 

serEputs ("M") ; 
itoa (si,  temp) ; 
serEputs (temp) ; 
temp [0]  =  ' \ 0  ' ; 

serEputs ("BD1") ;  //initializes  board  #  1 
serEputs ("SV2") ;  //sends  count  number  to  servo  2 

serEputs ("M") ; 

itoa(s2,  temp); 
serEputs (temp) ; 
temp [0]  =  ' \0 ' ; 

serEputs ("BD1") ;  //initializes  board  #  1 
serEputs ( "SV3" ) ;  //sends  count  number  to  servo  3 

serEputs ("M") ; 
itoa ( s3 ,  temp) ; 
serEputs (temp) ; 
temp [0]  =  ' \0 ' ; 


serEputs ("BD1") ;  //initializes  board  #  1 


serEputs ("SV4") ;  //sends 

serEputs ("M") ; 

itoa(s4,  temp); 
serEputs (temp) ; 
temp [0]  =  ' \0 ' ; 


count  number  to  servo  4 


serEputs ( "BD1 " ) ; 
serEputs ( "SV5 " ) ; 
serEputs ("M") ; 

itoa(s5,  temp); 
serEputs (temp) ; 
temp [0]  =  ' \0 ' ; 


//initializes  board  #  1 
/ / sends  count  number  to  servo  5 


serEputs ( "BD1" ) ;  //initializes  board  #  1 
serEputs ( "SV6" ) ;  //sends  count  number  to  servo  6 

serEputs ("M") ; 

itoa(s6,  temp) ; 
serEputs (temp) ; 
temp [0]  =  ' \0 ' ; 


serEputc ( ' \r ' ) ;  //needs  this  code  to  process  the  command 
temp [ 0 ]  =  ' \0  '  ; 

} 

tmemmap  xmem 


